void ins_update_gps(void) {
#ifdef USE_GPS
if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
if (!ins_ltp_initialised) {
ltp_def_from_ecef_i(&ins_ltp_def, &booz_gps_state.ecef_pos);
ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
ins_ltp_def.hmsl = booz_gps_state.hmsl;
ins_ltp_initialised = TRUE;
}
ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &booz_gps_state.ecef_pos);
ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &booz_gps_state.ecef_vel);
#ifdef USE_HFF
VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y);
VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y);
VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
if (ins_hf_realign) {
ins_hf_realign = FALSE;
#ifdef SITL
struct FloatVect2 true_pos, true_speed;
VECT2_COPY(true_pos, fdm.ltpprz_pos);
VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
b2_hff_realign(true_pos, true_speed);
#else
const struct FloatVect2 zero = {0.0, 0.0};
b2_hff_realign(ins_gps_pos_m_ned, zero);
#endif
}
b2_hff_update_gps();
#ifndef USE_VFF /* vff not used */
ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
ins_ltp_speed.z = (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
#endif /* vff not used */
#endif /* hff used */
#ifndef USE_HFF /* hff not used */
#ifndef USE_VFF /* neither hf nor vf used */
INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#else /* only vff used */
INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif
#ifdef USE_GPS_LAG_HACK
VECT2_COPY(d_pos, ins_ltp_speed);
INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
VECT2_ADD(ins_ltp_pos, d_pos);
#endif
#endif /* hff not used */
INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
}
#endif /* USE_GPS */
}
void ins_update_gps(void) {
#if USE_GPS
//Check for GPS fix
if (gps.fix == GPS_FIX_3D) {
//Set the initial coordinates
if(!ins_impl.ltp_initialized) {
ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos);
ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
ins_impl.ltp_def.hmsl = gps.hmsl;
ins_impl.ltp_initialized = TRUE;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
}
//Set the x and y and maybe z position in ltp and save
struct NedCoor_i ins_gps_pos_cm_ned;
ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos);
//When we don't want to use the height of the navdata we can use the gps height
#if USE_GPS_HEIGHT
INT32_VECT3_SCALE_2(ins_impl.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
#else
INT32_VECT2_SCALE_2(ins_impl.ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
#endif
//Set the local origin
stateSetPositionNed_i(&ins_impl.ltp_pos);
}
#endif /* USE_GPS */
}
void ins_update_gps(void) {
#if USE_GPS
if (gps.fix == GPS_FIX_3D) {
if (!ins_ltp_initialised) {
ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos);
ins_ltp_def.lla.alt = gps.lla_pos.alt;
ins_ltp_def.hmsl = gps.hmsl;
ins_ltp_initialised = TRUE;
stateSetLocalOrigin_i(&ins_ltp_def);
}
ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel);
#if USE_HFF
VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y);
VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y);
VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
if (ins.hf_realign) {
ins.hf_realign = FALSE;
#ifdef SITL
struct FloatVect2 true_pos, true_speed;
VECT2_COPY(true_pos, fdm.ltpprz_pos);
VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
b2_hff_realign(true_pos, true_speed);
#else
const struct FloatVect2 zero = {0.0, 0.0};
b2_hff_realign(ins_gps_pos_m_ned, zero);
#endif
}
b2_hff_update_gps();
#endif /* hff used */
#if !USE_HFF /* hff not used */
INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif /* hff not used */
}
#endif /* USE_GPS */
}
void dl_parse_msg(void) {
datalink_time = 0;
uint8_t msg_id = IdOfMsg(dl_buffer);
switch (msg_id) {
case DL_PING:
{
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
}
break;
case DL_SETTING :
{
if (DL_SETTING_ac_id(dl_buffer) != AC_ID) break;
uint8_t i = DL_SETTING_index(dl_buffer);
float var = DL_SETTING_value(dl_buffer);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
}
break;
case DL_GET_SETTING :
{
if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) break;
uint8_t i = DL_GET_SETTING_index(dl_buffer);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
break;
/* this case for DL_HOVER_POINT_CHANGE_ECEF is added by Yi and Edward */
/*********************************************************************/
case DL_HOVER_POINT_CHANGE_ECEF:
{
uint8_t ac_id = DL_HOVER_POINT_CHANGE_ECEF_ac_id(dl_buffer);
if(ac_id != AC_ID) break;
uint8_t external_gps_fixed = DL_HOVER_POINT_CHANGE_ECEF_external_gps_fix(dl_buffer);
/** getting the ecef coordinate from external gps **/
struct EcefCoor_i external_gps_ecef_pos;
external_gps_ecef_pos.x = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_x(dl_buffer);//DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_x() is defined in dl_protocol.h
external_gps_ecef_pos.y = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_y(dl_buffer);
external_gps_ecef_pos.z = DL_HOVER_POINT_CHANGE_ECEF_ecef_pos_z(dl_buffer);
DOWNLINK_SEND_HOVER_POINT_CHANGE_ECEF(DefaultChannel, DefaultDevice, &ac_id, &external_gps_fixed, &(external_gps_ecef_pos.x), &(external_gps_ecef_pos.y), &(external_gps_ecef_pos.z));
if(external_gps_fixed == 0x03)//case for 3D_Fix.
{
if(!ins_ltp_initialised || guidance_h_mode != GUIDANCE_H_MODE_HOVER) break;//ins_ltp_initialised defined in ins_int.h
//no HMOE reference
//not in the HOVER_Z_HOLD mode
//not in flight(looks like no needed)
struct NedCoor_i external_gps_ned_pos;
memset((void *) &external_gps_ned_pos,0,sizeof(struct NedCoor_i));//just in case, clear the memory.
/** convert the ecef coordinate into ned(ltp) coordinate **/
ned_of_ecef_point_i(&external_gps_ned_pos,&ins_ltp_def,&external_gps_ecef_pos);
/** change the current setpoint with external_gps_ned_pos coordinate **/
struct Int32Vect2 external_gps_h_pos;
//cm to m and store in external_gps_h_pos
//check function ins_update_gps() for reference.
INT32_VECT2_SCALE_2(external_gps_h_pos,external_gps_ned_pos,INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
//subsitute the guidance_h_pos_sp
VECT2_COPY(guidance_h_pos_sp,external_gps_h_pos);
/*
* the above code is used for congruity, but waste space and time.
* simple using
*
* guidance_h_pos_sp.x = external_gps_ned_pos.x;
* guidance_h_pos_sp.y = external_gps_ned_pox.y;
*
* can achieve the same effect.
*/
}
else break;//hover pos_sp no change
}
/**------------------------------------------------------------------*/
#if defined USE_NAVIGATION
case DL_BLOCK :
{
if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) break;
nav_goto_block(DL_BLOCK_block_id(dl_buffer));
}
break;
case DL_MOVE_WP :
{
uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
if (ac_id != AC_ID) break;
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
struct LlaCoor_i lla;
struct EnuCoor_i enu;
lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer));
lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer));
/* WP_alt is in cm, lla.alt in mm */
lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
enu.x = POS_BFP_OF_REAL(enu.x)/100;
enu.y = POS_BFP_OF_REAL(enu.y)/100;
enu.z = POS_BFP_OF_REAL(enu.z)/100;
VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
}
break;
#endif /* USE_NAVIGATION */
#ifdef RADIO_CONTROL_TYPE_DATALINK
case DL_RC_3CH :
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_3ch_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
break;
case DL_RC_4CH :
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_4ch_datalink(
DL_RC_4CH_mode(dl_buffer),
DL_RC_4CH_throttle(dl_buffer),
DL_RC_4CH_roll(dl_buffer),
DL_RC_4CH_pitch(dl_buffer),
DL_RC_4CH_yaw(dl_buffer));
break;
#endif // RADIO_CONTROL_TYPE_DATALINK
default:
break;
}
/* Parse modules datalink */
modules_parse_datalink(msg_id);
}
void ins_reset_altitude_ref(void)
{
#if USE_GPS
struct LlaCoor_i lla = {
.lat = state.ned_origin_i.lla.lat,
.lon = state.ned_origin_i.lla.lon,
.alt = gps.lla_pos.alt
};
ltp_def_from_lla_i(&ins_impl.ltp_def, &lla);
ins_impl.ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
#endif
ins_impl.vf_reset = TRUE;
}
void ins_propagate(float dt)
{
/* untilt accels */
struct Int32Vect3 accel_meas_body;
struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel);
struct Int32Vect3 accel_meas_ltp;
int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body);
float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
if (ins_impl.baro_initialized) {
vff_propagate(z_accel_meas_float, dt);
ins_update_from_vff();
} else { // feed accel from the sensors
// subtract -9.81m/s2 (acceleration measured due to gravity,
// but vehicle not accelerating in ltp)
ins_impl.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
}
#if USE_HFF
/* propagate horizontal filter */
b2_hff_propagate();
/* convert and copy result to ins_impl */
ins_update_from_hff();
#else
ins_impl.ltp_accel.x = accel_meas_ltp.x;
ins_impl.ltp_accel.y = accel_meas_ltp.y;
#endif /* USE_HFF */
ins_ned_to_state();
}
static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure)
{
if (!ins_impl.baro_initialized && *pressure > 1e-7) {
// wait for a first positive value
ins_impl.qfe = *pressure;
ins_impl.baro_initialized = TRUE;
}
if (ins_impl.baro_initialized) {
if (ins_impl.vf_reset) {
ins_impl.vf_reset = FALSE;
ins_impl.qfe = *pressure;
vff_realign(0.);
ins_update_from_vff();
} else {
ins_impl.baro_z = -pprz_isa_height_of_pressure(*pressure, ins_impl.qfe);
#if USE_VFF_EXTENDED
vff_update_baro(ins_impl.baro_z);
#else
vff_update(ins_impl.baro_z);
#endif
}
ins_ned_to_state();
}
}
#if USE_GPS
void ins_update_gps(void)
{
if (gps.fix == GPS_FIX_3D) {
if (!ins_impl.ltp_initialized) {
ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos);
ins_impl.ltp_def.lla.alt = gps.lla_pos.alt;
ins_impl.ltp_def.hmsl = gps.hmsl;
ins_impl.ltp_initialized = TRUE;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
}
struct NedCoor_i gps_pos_cm_ned;
ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos);
/// @todo maybe use gps.ned_vel directly??
struct NedCoor_i gps_speed_cm_s_ned;
ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_impl.ltp_def, &gps.ecef_vel);
#if INS_USE_GPS_ALT
vff_update_z_conf((float)gps_pos_cm_ned.z / 100.0, INS_VFF_R_GPS);
#endif
#if USE_HFF
/* horizontal gps transformed to NED in meters as float */
struct FloatVect2 gps_pos_m_ned;
VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y);
VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f);
struct FloatVect2 gps_speed_m_s_ned;
VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.);
if (ins_impl.hf_realign) {
ins_impl.hf_realign = FALSE;
const struct FloatVect2 zero = {0.0f, 0.0f};
b2_hff_realign(gps_pos_m_ned, zero);
}
// run horizontal filter
b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
// convert and copy result to ins_impl
ins_update_from_hff();
#else /* hff not used */
/* simply copy horizontal pos/speed from gps */
INT32_VECT2_SCALE_2(ins_impl.ltp_pos, gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT2_SCALE_2(ins_impl.ltp_speed, gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif /* USE_HFF */
ins_ned_to_state();
}
}
#endif /* USE_GPS */
#if USE_SONAR
static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *distance)
{
static float last_offset = 0.;
/* update filter assuming a flat ground */
if (*distance < INS_SONAR_MAX_RANGE && *distance > INS_SONAR_MIN_RANGE
#ifdef INS_SONAR_THROTTLE_THRESHOLD
&& stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
#endif
#ifdef INS_SONAR_BARO_THRESHOLD
&& ins_impl.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */
#endif
&& ins_impl.update_on_agl
&& ins_impl.baro_initialized) {
vff_update_z_conf(-(*distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(*distance));
last_offset = vff.offset;
} else {
/* update offset with last value to avoid divergence */
vff_update_offset(last_offset);
}
}
#endif // USE_SONAR
/** initialize the local origin (ltp_def) from flight plan position */
static void ins_init_origin_from_flightplan(void)
{
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = NAV_LAT0;
llh_nav0.lon = NAV_LON0;
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
struct EcefCoor_i ecef_nav0;
ecef_of_lla_i(&ecef_nav0, &llh_nav0);
ltp_def_from_ecef_i(&ins_impl.ltp_def, &ecef_nav0);
ins_impl.ltp_def.hmsl = NAV_ALT0;
stateSetLocalOrigin_i(&ins_impl.ltp_def);
}
/** copy position and speed to state interface */
static void ins_ned_to_state(void)
{
stateSetPositionNed_i(&ins_impl.ltp_pos);
stateSetSpeedNed_i(&ins_impl.ltp_speed);
stateSetAccelNed_i(&ins_impl.ltp_accel);
#if defined SITL && USE_NPS
if (nps_bypass_ins) {
sim_overwrite_ins();
}
#endif
}
void ins_reset_altitude_ref(void)
{
#if USE_GPS
struct LlaCoor_i lla = {
.lat = state.ned_origin_i.lla.lat,
.lon = state.ned_origin_i.lla.lon,
.alt = gps.lla_pos.alt
};
ltp_def_from_lla_i(&ins_int.ltp_def, &lla);
ins_int.ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ins_int.ltp_def);
#endif
ins_int.vf_reset = TRUE;
}
void ins_int_propagate(struct Int32Vect3 *accel, float dt)
{
/* untilt accels */
struct Int32Vect3 accel_meas_body;
struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, accel);
struct Int32Vect3 accel_meas_ltp;
int32_rmat_transp_vmult(&accel_meas_ltp, stateGetNedToBodyRMat_i(), &accel_meas_body);
float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
/* Propagate only if we got any measurement during the last INS_MAX_PROPAGATION_STEPS.
* Otherwise halt the propagation to not diverge and only set the acceleration.
* This should only be relevant in the startup phase when the baro is not yet initialized
* and there is no gps fix yet...
*/
if (ins_int.propagation_cnt < INS_MAX_PROPAGATION_STEPS) {
vff_propagate(z_accel_meas_float, dt);
ins_update_from_vff();
} else {
// feed accel from the sensors
// subtract -9.81m/s2 (acceleration measured due to gravity,
// but vehicle not accelerating in ltp)
ins_int.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
}
#if USE_HFF
/* propagate horizontal filter */
b2_hff_propagate();
/* convert and copy result to ins_int */
ins_update_from_hff();
#else
ins_int.ltp_accel.x = accel_meas_ltp.x;
ins_int.ltp_accel.y = accel_meas_ltp.y;
#endif /* USE_HFF */
ins_ned_to_state();
/* increment the propagation counter, while making sure it doesn't overflow */
if (ins_int.propagation_cnt < 100 * INS_MAX_PROPAGATION_STEPS) {
ins_int.propagation_cnt++;
}
}
static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
{
if (!ins_int.baro_initialized && pressure > 1e-7) {
// wait for a first positive value
ins_int.qfe = pressure;
ins_int.baro_initialized = TRUE;
}
if (ins_int.baro_initialized) {
if (ins_int.vf_reset) {
ins_int.vf_reset = FALSE;
ins_int.qfe = pressure;
vff_realign(0.);
ins_update_from_vff();
} else {
ins_int.baro_z = -pprz_isa_height_of_pressure(pressure, ins_int.qfe);
#if USE_VFF_EXTENDED
vff_update_baro(ins_int.baro_z);
#else
vff_update(ins_int.baro_z);
#endif
}
ins_ned_to_state();
/* reset the counter to indicate we just had a measurement update */
ins_int.propagation_cnt = 0;
}
}
#if USE_GPS
void ins_int_update_gps(struct GpsState *gps_s)
{
if (gps_s->fix < GPS_FIX_3D) {
return;
}
if (!ins_int.ltp_initialized) {
ins_reset_local_origin();
}
struct NedCoor_i gps_pos_cm_ned;
ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &gps_s->ecef_pos);
/* calculate body frame position taking BODY_TO_GPS translation (in cm) into account */
#ifdef INS_BODY_TO_GPS_X
/* body2gps translation in body frame */
struct Int32Vect3 b2g_b = {
.x = INS_BODY_TO_GPS_X,
.y = INS_BODY_TO_GPS_Y,
.z = INS_BODY_TO_GPS_Z
};
/* rotate offset given in body frame to navigation/ltp frame using current attitude */
struct Int32Quat q_b2n = *stateGetNedToBodyQuat_i();
QUAT_INVERT(q_b2n, q_b2n);
struct Int32Vect3 b2g_n;
int32_quat_vmult(&b2g_n, &q_b2n, &b2g_b);
/* subtract body2gps translation in ltp from gps position */
VECT3_SUB(gps_pos_cm_ned, b2g_n);
#endif
/// @todo maybe use gps_s->ned_vel directly??
struct NedCoor_i gps_speed_cm_s_ned;
ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &gps_s->ecef_vel);
#if INS_USE_GPS_ALT
vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS);
#endif
#if INS_USE_GPS_ALT_SPEED
vff_update_vz_conf(((float)gps_speed_cm_s_ned.z) / 100.0, INS_VFF_VZ_R_GPS);
#endif
#if USE_HFF
/* horizontal gps transformed to NED in meters as float */
struct FloatVect2 gps_pos_m_ned;
VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y);
VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f);
struct FloatVect2 gps_speed_m_s_ned;
VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.);
if (ins_int.hf_realign) {
ins_int.hf_realign = FALSE;
const struct FloatVect2 zero = {0.0f, 0.0f};
b2_hff_realign(gps_pos_m_ned, zero);
}
// run horizontal filter
b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
// convert and copy result to ins_int
ins_update_from_hff();
#else /* hff not used */
/* simply copy horizontal pos/speed from gps */
INT32_VECT2_SCALE_2(ins_int.ltp_pos, gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT2_SCALE_2(ins_int.ltp_speed, gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif /* USE_HFF */
ins_ned_to_state();
/* reset the counter to indicate we just had a measurement update */
ins_int.propagation_cnt = 0;
}
#else
void ins_int_update_gps(struct GpsState *gps_s __attribute__((unused))) {}
#endif /* USE_GPS */
#if USE_SONAR
static void sonar_cb(uint8_t __attribute__((unused)) sender_id, float distance)
{
static float last_offset = 0.;
/* update filter assuming a flat ground */
if (distance < INS_SONAR_MAX_RANGE && distance > INS_SONAR_MIN_RANGE
#ifdef INS_SONAR_THROTTLE_THRESHOLD
&& stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
#endif
#ifdef INS_SONAR_BARO_THRESHOLD
&& ins_int.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */
#endif
&& ins_int.update_on_agl
&& ins_int.baro_initialized) {
vff_update_z_conf(-(distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(distance));
last_offset = vff.offset;
} else {
/* update offset with last value to avoid divergence */
vff_update_offset(last_offset);
}
/* reset the counter to indicate we just had a measurement update */
ins_int.propagation_cnt = 0;
}
#endif // USE_SONAR
/** initialize the local origin (ltp_def) from flight plan position */
static void ins_init_origin_from_flightplan(void)
{
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = NAV_LAT0;
llh_nav0.lon = NAV_LON0;
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
struct EcefCoor_i ecef_nav0;
ecef_of_lla_i(&ecef_nav0, &llh_nav0);
ltp_def_from_ecef_i(&ins_int.ltp_def, &ecef_nav0);
ins_int.ltp_def.hmsl = NAV_ALT0;
stateSetLocalOrigin_i(&ins_int.ltp_def);
}
/** copy position and speed to state interface */
static void ins_ned_to_state(void)
{
stateSetPositionNed_i(&ins_int.ltp_pos);
stateSetSpeedNed_i(&ins_int.ltp_speed);
stateSetAccelNed_i(&ins_int.ltp_accel);
#if defined SITL && USE_NPS
if (nps_bypass_ins) {
sim_overwrite_ins();
}
#endif
}