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); }