/**Send by downlink the GPS and rad_of_ir messages with \a DOWNLINK_SEND_GPS
* and \a DOWNLINK_SEND_RAD_OF_IR \n
* If \a estimator_flight_time is null and \a estimator_hspeed_mod is greater
* than \a MIN_SPEED_FOR_TAKEOFF, set the \a estimator_flight_time to 1 and \a
* launch to true (which is not set in non auto launch. Then call
* \a DOWNLINK_SEND_TAKEOFF
*/
void use_gps_pos( void ) {
if (GpsFixValid()) {
last_gps_msg_t = cpu_time_sec;
estimator_update_state_gps();
}
DOWNLINK_SEND_GPS(&gps_mode, &gps_utm_east, &gps_utm_north, &gps_course, &gps_alt, &gps_gspeed,&gps_climb, &gps_itow, &gps_utm_zone, &gps_nb_ovrn);
static uint8_t i;
if (i == gps_nb_channels) i = 0;
if (i < gps_nb_channels && gps_svinfos[i].cno > 0)
DOWNLINK_SEND_SVINFO(&i, &gps_svinfos[i].svid, &gps_svinfos[i].flags, &gps_svinfos[i].qi, &gps_svinfos[i].cno, &gps_svinfos[i].elev, &gps_svinfos[i].azim);
i++;
}
void alt_filter_periodic(void) {
// estimation at each step
kalmanEstimation(&alt_filter,0.);
// update on new data
float ga = (float)gps.hmsl / 1000.;
if (baro_ets_altitude != last_baro_alt) {
kalmanCorrectionAltimetre(&alt_filter, baro_ets_altitude);
last_baro_alt = baro_ets_altitude;
}
if (ga != last_gps_alt && GpsFixValid()) {
kalmanCorrectionGPS(&alt_filter, ga);
last_gps_alt = ga;
}
RunOnceEvery(6,DOWNLINK_SEND_VFF(DefaultChannel, &baro_ets_altitude,
&(alt_filter.X[0]), &(alt_filter.X[1]), &(alt_filter.X[2]),
&(alt_filter.P[0][0]), &(alt_filter.P[1][1]), &(alt_filter.P[2][2])));
}
void ins_reset_local_origin(void)
{
#if USE_GPS
if (GpsFixValid()) {
ltp_def_from_ecef_i(&ins_int.ltp_def, &gps.ecef_pos);
ins_int.ltp_def.lla.alt = gps.lla_pos.alt;
ins_int.ltp_def.hmsl = gps.hmsl;
ins_int.ltp_initialized = TRUE;
stateSetLocalOrigin_i(&ins_int.ltp_def);
} else {
ins_int.ltp_initialized = FALSE;
}
#else
ins_int.ltp_initialized = FALSE;
#endif
#if USE_HFF
ins_int.hf_realign = TRUE;
#endif
ins_int.vf_reset = TRUE;
}
void ahrs_chimu_update_gps(uint8_t gps_fix __attribute__((unused)), uint16_t gps_speed_3d)
{
// Send SW Centripetal Corrections
uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 };
float gps_speed = 0;
if (GpsFixValid()) {
gps_speed = gps_speed_3d / 100.;
}
gps_speed = FloatSwap(gps_speed);
memmove(¢ripedal[6], &gps_speed, 4);
// Fill X-speed
CHIMU_Checksum(centripedal, 19);
InsSend(centripedal, 19);
// Downlink Send
}
