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