radar_kin_utils.h

Go to the documentation of this file.

/*
 *
 * Copyright (c) 2018 Texas Instruments Incorporated
 *
 * All rights reserved not granted herein.
 *
 * Limited License.
 *
 * Texas Instruments Incorporated grants a world-wide, royalty-free, non-exclusive
 * license under copyrights and patents it now or hereafter owns or controls to make,
 * have made, use, import, offer to sell and sell ("Utilize") this software subject to the
 * terms herein.  With respect to the foregoing patent license, such license is granted
 * solely to the extent that any such patent is necessary to Utilize the software alone.
 * The patent license shall not apply to any combinations which include this software,
 * other than combinations with devices manufactured by or for TI ("TI Devices").
 * No hardware patent is licensed hereunder.
 *
 * Redistributions must preserve existing copyright notices and reproduce this license
 * (including the above copyright notice and the disclaimer and (if applicable) source
 * code license limitations below) in the documentation and/or other materials provided
 * with the distribution
 *
 * Redistribution and use in binary form, without modification, are permitted provided
 * that the following conditions are met:
 *
 * *       No reverse engineering, decompilation, or disassembly of this software is
 * permitted with respect to any software provided in binary form.
 *
 * *       any redistribution and use are licensed by TI for use only with TI Devices.
 *
 * *       Nothing shall obligate TI to provide you with source code for the software
 * licensed and provided to you in object code.
 *
 * If software source code is provided to you, modification and redistribution of the
 * source code are permitted provided that the following conditions are met:
 *
 * *       any redistribution and use of the source code, including any resulting derivative
 * works, are licensed by TI for use only with TI Devices.
 *
 * *       any redistribution and use of any object code compiled from the source code
 * and any resulting derivative works, are licensed by TI for use only with TI Devices.
 *
 * Neither the name of Texas Instruments Incorporated nor the names of its suppliers
 *
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * DISCLAIMER.
 *
 * THIS SOFTWARE IS PROVIDED BY TI AND TI'S LICENSORS "AS IS" AND ANY EXPRESS
 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL TI AND TI'S LICENSORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
 * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
 * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 * OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */
#ifndef RADAR_KIN_UTILS_H
#define RADAR_KIN_UTILS_H

#include <perception/perception.h>
#include <perception/mathmapping.h>

#ifdef __cplusplus
extern "C" {
#endif

#define IMU_TIME_TO_SECONDS_CONVERSION_CONSTANT      (1000000.0f)

/* PUBLIC APIS  */
static inline float PTK_Util_computeYawRate(const PTK_INS_Record  * prevInsRec,
                                            const PTK_INS_Record  * curInsRec)
{
    float   yawRate;

    yawRate = IMU_TIME_TO_SECONDS_CONVERSION_CONSTANT *
              (curInsRec->data.inspva.attitude.azimuth -
               prevInsRec->data.inspva.attitude.azimuth) /
              (float)(curInsRec->timestamp - prevInsRec->timestamp);

    return yawRate;
}

void PTK_Util_KinCompRotMatrices(
    float          * rotAngle_H_M,
    float          * rotAngle_H,
    const float      azimuth,
    const float      sensorOrientation,
    float          * rotMat_H_M,
    float          * rotMat_H);

void PTK_Util_KinCompSensorVel(
    float  * v_sensor,                               
    float  * rotMat_H,                             
    const float    yawRate,
    const float    velEast,
    const float    velNorth,
    const float    sensorPosX,
    const float    sensorPosY);

void PTK_Util_KinTfrmSensorLoc(
    const float  * rotMat_H,
    const float    sensorPosX,
    const float    sensorPosY,
    float        * trfLoc_x,                              
    float        * trfLoc_y);

void PTK_Util_KinCompSensorPva(                           
    const float                rotAngle_H_M,                          
    const PTK_RigidTransform * M_w_enu,                   
    const float                trfLoc_x,          
    const float                trfLoc_y,
    float                    * sensor_pva);

void PTK_Util_KinCompRadVelWR2WFromSensorOrigin(
    const float   rotAngle_H_M,
    const float * v_s_w,
    const float   vRad_o_s,
    const float   a_o_s,
    const float   rotAngle_H,
    float       * vRad_o_sw);


void PTK_Util_KinCompRadVelWR2WFromWorldOrigin(
    const float   rotAngle_H_M,
    const float * v_s_w,
    const float   vRad_o_s,
    const float   a_o_s,
    const float   aRad_o_w,
    const float * sensor_pva,
    const float   x_o_w,
    const float   y_o_w,
    const float   rotAngle_H,
    float       * vRad_o_w);

#ifdef __cplusplus
}
#endif

#endif