void ins_reset_altitude_ref(void)
{
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_gp.ltp_def, &lla);
ins_gp.ltp_def.hmsl = gps.hmsl;
stateSetLocalOrigin_i(&ins_gp.ltp_def);
}
#include "subsystems/abi.h"
static abi_event gps_ev;
static void gps_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct GpsState *gps_s)
{
if (gps_s->fix == GPS_FIX_3D) {
if (!ins_gp.ltp_initialized) {
ins_reset_local_origin();
}
/* simply scale and copy pos/speed from gps */
struct NedCoor_i gps_pos_cm_ned;
ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &gps_s->ecef_pos);
INT32_VECT3_SCALE_2(ins_gp.ltp_pos, gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
stateSetPositionNed_i(&ins_gp.ltp_pos);
struct NedCoor_i gps_speed_cm_s_ned;
ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &gps_s->ecef_vel);
INT32_VECT3_SCALE_2(ins_gp.ltp_speed, gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
stateSetSpeedNed_i(&ins_gp.ltp_speed);
}
}
void ins_gps_passthrough_register(void);
{
ins_register_impl(ins_gps_passthrough_init);
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
}
void ins_vectornav_register(void)
{
ins_register_impl(ins_vectornav_init);
}
void ins_xsens_register(void)
{
ins_register_impl(ins_xsens_init);
AbiBindMsgGPS(INS_XSENS700_GPS_ID, &gps_ev, gps_cb);
}
void ins_xsens_register(void)
{
ins_register_impl(ins_xsens_init);
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
}
