void ins_update_baro() {
#if USE_BAROMETER
// TODO update kalman filter with baro struct
if (baro.status == BS_RUNNING) {
if (!ins_baro_initialised) {
ins_qfe = baro.absolute;
ins_baro_initialised = TRUE;
}
if (ins.vf_realign) {
ins.vf_realign = FALSE;
ins_qfe = baro.absolute;
}
else { /* not realigning, so normal update with baro measurement */
/* altitude decreases with increasing baro.absolute pressure */
ins_baro_alt = ground_alt - (baro.absolute - ins_qfe) * INS_BARO_SENS;
/* run the filter */
EstimatorSetAlt(ins_baro_alt);
/* set new altitude, just copy old horizontal position */
struct UtmCoor_f utm;
UTM_COPY(utm, *stateGetPositionUtm_f());
utm.alt = ins_alt;
stateSetPositionUtm_f(&utm);
}
}
#endif
}
struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
{
struct UtmCoor_f utm = {.east = 0., .north=0., .alt=0., .zone=zone};
if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
/* A real UTM position is available, use the correct zone */
UTM_FLOAT_OF_BFP(utm, gps_s->utm_pos);
} else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT))
{
/* Recompute UTM coordinates in this zone */
struct UtmCoor_i utm_i;
utm_i.zone = zone;
utm_of_lla_i(&utm_i, &gps_s->lla_pos);
UTM_FLOAT_OF_BFP(utm, utm_i);
/* set utm.alt in hsml */
if (bit_is_set(gps_s->valid_fields, GPS_VALID_HMSL_BIT)) {
utm.alt = gps_s->hmsl/1000.;
} else {
utm.alt = wgs84_ellipsoid_to_geoid_i(gps_s->lla_pos.lat, gps_s->lla_pos.lon)/1000.;
}
}
return utm;
}
struct UtmCoor_i utm_int_from_gps(struct GpsState *gps_s, uint8_t zone)
{
struct UtmCoor_i utm = {.east = 0, .north=0, .alt=0, .zone=zone};
if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
// A real UTM position is available, use the correct zone
UTM_COPY(utm, gps_s->utm_pos);
}
else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)){
/* Recompute UTM coordinates in zone */
utm_of_lla_i(&utm, &gps_s->lla_pos);
/* set utm.alt in hsml */
if (bit_is_set(gps_s->valid_fields, GPS_VALID_HMSL_BIT)) {
utm.alt = gps_s->hmsl;
} else {
utm.alt = wgs84_ellipsoid_to_geoid_i(gps_s->lla_pos.lat, gps_s->lla_pos.lon);
}
}
return utm;
}
