void gps_init(void)
{
multi_gps_mode = MULTI_GPS_MODE;
gps.valid_fields = 0;
gps.fix = GPS_FIX_NONE;
gps.week = 0;
gps.tow = 0;
gps.cacc = 0;
gps.last_3dfix_ticks = 0;
gps.last_3dfix_time = 0;
gps.last_msg_ticks = 0;
gps.last_msg_time = 0;
#ifdef GPS_POWER_GPIO
gpio_setup_output(GPS_POWER_GPIO);
GPS_POWER_GPIO_ON(GPS_POWER_GPIO);
#endif
#ifdef GPS_LED
LED_OFF(GPS_LED);
#endif
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS, send_gps);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_INT, send_gps_int);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_LLA, send_gps_lla);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_SOL, send_gps_sol);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SVINFO, send_svinfo);
#endif
}
void ahrs_infrared_init(void)
{
heading = 0.;
AbiBindMsgIMU_GYRO_INT32(AHRS_INFRARED_GYRO_ID, &gyro_ev, gyro_cb);
AbiBindMsgGPS(AHRS_INFRARED_GPS_ID, &gps_ev, &gps_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "IR_SENSORS", send_infrared);
register_periodic_telemetry(DefaultPeriodic, "STATE_FILTER_STATUS", send_status);
#endif
}
void intermcu_init(void)
{
pprz_transport_init(&intermcu.transport);
#if USE_GPS
AbiBindMsgGPS(IMCU_GPS_ID, &gps_ev, gps_cb);
#endif
#ifdef BOARD_PX4IO
px4bl_tid = sys_time_register_timer(10.0, NULL);
#endif
}
void ins_xsens700_init(void)
{
xsens700_init();
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
stateSetLocalUtmOrigin_f(&utm0);
stateSetPositionUtm_f(&utm0);
AbiBindMsgGPS(INS_XSENS700_GPS_ID, &gps_ev, gps_cb);
}
void ins_int_init(void)
{
#if USE_INS_NAV_INIT
ins_init_origin_i_from_flightplan(&ins_int.ltp_def);
ins_int.ltp_initialized = true;
#else
ins_int.ltp_initialized = false;
#endif
/* we haven't had any measurement updates yet, so set the counter to max */
ins_int.propagation_cnt = INS_MAX_PROPAGATION_STEPS;
// Bind to BARO_ABS message
AbiBindMsgBARO_ABS(INS_INT_BARO_ID, &baro_ev, baro_cb);
ins_int.baro_initialized = false;
#if USE_SONAR
ins_int.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
// Bind to AGL message
AbiBindMsgAGL(INS_INT_SONAR_ID, &sonar_ev, sonar_cb);
#endif
ins_int.vf_reset = false;
ins_int.hf_realign = false;
/* init vertical and horizontal filters */
vff_init_zero();
#if USE_HFF
hff_init(0., 0., 0., 0.);
#endif
INT32_VECT3_ZERO(ins_int.ltp_pos);
INT32_VECT3_ZERO(ins_int.ltp_speed);
INT32_VECT3_ZERO(ins_int.ltp_accel);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS, send_ins);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_Z, send_ins_z);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref);
#endif
/*
* Subscribe to scaled IMU measurements and attach callbacks
*/
AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb);
AbiBindMsgPOSITION_ESTIMATE(INS_INT_POS_ID, &pos_est_ev, pos_est_cb);
}
void gps_init(void)
{
multi_gps_mode = MULTI_GPS_MODE;
gps.valid_fields = 0;
gps.fix = GPS_FIX_NONE;
gps.week = 0;
gps.tow = 0;
gps.cacc = 0;
gps.last_3dfix_ticks = 0;
gps.last_3dfix_time = 0;
gps.last_msg_ticks = 0;
gps.last_msg_time = 0;
#ifdef GPS_POWER_GPIO
gpio_setup_output(GPS_POWER_GPIO);
GPS_POWER_GPIO_ON(GPS_POWER_GPIO);
#endif
#ifdef GPS_LED
LED_OFF(GPS_LED);
#endif
RegisterGps(PRIMARY_GPS);
#ifdef SECONDARY_GPS
RegisterGps(SECONDARY_GPS);
#endif
for (int i=0; i < GPS_NB_IMPL; i++) {
if (GpsInstances[i].init != NULL) {
GpsInstances[i].init();
}
}
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS, send_gps);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_INT, send_gps_int);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_LLA, send_gps_lla);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_SOL, send_gps_sol);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SVINFO, send_svinfo);
#endif
}
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_module_wrapper_init(void)
{
#if USE_INS_NAV_INIT
ins_init_origin_i_from_flightplan(&ins_module.ltp_def);
ins_module.ltp_initialized = true;
#else
ins_module.ltp_initialized = false;
#endif
INT32_VECT3_ZERO(ins_module.ltp_pos);
INT32_VECT3_ZERO(ins_module.ltp_speed);
INT32_VECT3_ZERO(ins_module.ltp_accel);
ins_module_init();
// Bind to ABI messages
AbiBindMsgBARO_ABS(INS_MODULE_BARO_ID, &baro_ev, baro_cb);
AbiBindMsgIMU_ACCEL_INT32(INS_MODULE_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgGPS(INS_MODULE_GPS_ID, &gps_ev, gps_cb);
AbiBindMsgBODY_TO_IMU_QUAT(INS_MODULE_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
}
void ins_xsens_register(void)
{
ins_register_impl(ins_xsens_init);
AbiBindMsgGPS(INS_XSENS700_GPS_ID, &gps_ev, gps_cb);
}
void ahrs_chimu_register(void)
{
ahrs_chimu_init();
ahrs_register_impl(ahrs_chimu_enable_output);
AbiBindMsgGPS(AHRS_CHIMU_GPS_ID, &gps_ev, gps_cb);
}
void ins_xsens_register(void)
{
ins_register_impl(ins_xsens_init);
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
}
