void AP_ADSB::send_dynamic_out(const mavlink_channel_t chan) { const AP_GPS &gps = AP::gps(); const Vector3f &gps_velocity = gps.velocity(); const int32_t latitude = _my_loc.lat; const int32_t longitude = _my_loc.lng; const int32_t altGNSS = _my_loc.alt * 10; // convert cm to mm const int16_t velVert = gps_velocity.z * 1E2; // convert m/s to cm/s const int16_t nsVog = gps_velocity.x * 1E2; // convert m/s to cm/s const int16_t ewVog = gps_velocity.y * 1E2; // convert m/s to cm/s const uint8_t fixType = gps.status(); // this lines up perfectly with our enum const uint8_t emStatus = 0; // TODO: implement this ENUM. no emergency = 0 const uint8_t numSats = gps.num_sats(); const uint16_t squawk = out_state.cfg.squawk_octal; uint32_t accHoriz = UINT_MAX; float accHoriz_f; if (gps.horizontal_accuracy(accHoriz_f)) { accHoriz = accHoriz_f * 1E3; // convert m to mm } uint16_t accVert = USHRT_MAX; float accVert_f; if (gps.vertical_accuracy(accVert_f)) { accVert = accVert_f * 1E2; // convert m to cm } uint16_t accVel = USHRT_MAX; float accVel_f; if (gps.speed_accuracy(accVel_f)) { accVel = accVel_f * 1E3; // convert m/s to mm/s } uint16_t state = 0; if (out_state._is_in_auto_mode) { state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_AUTOPILOT_ENABLED; } if (!out_state.is_flying) { state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_ON_GROUND; } // TODO: confirm this sets utcTime correctly const uint64_t gps_time = gps.time_epoch_usec(); const uint32_t utcTime = gps_time / 1000000ULL; const AP_Baro &baro = AP::baro(); int32_t altPres = INT_MAX; if (baro.healthy()) { // Altitude difference between sea level pressure and current pressure. Result in millimeters altPres = baro.get_altitude_difference(SSL_AIR_PRESSURE, baro.get_pressure()) * 1E3; // convert m to mm; } mavlink_msg_uavionix_adsb_out_dynamic_send( chan, utcTime, latitude, longitude, altGNSS, fixType, numSats, altPres, accHoriz, accVert, accVel, velVert, nsVog, ewVog, emStatus, state, squawk); }
void AP_ADSB::send_dynamic_out(const mavlink_channel_t chan) { // -------------- // Knowns AP_GPS gps = _ahrs.get_gps(); Vector3f gps_velocity = gps.velocity(); int32_t latitude = _my_loc.lat; int32_t longitude = _my_loc.lng; int32_t altGNSS = _my_loc.alt*0.1f; // convert cm to mm int16_t velVert = gps_velocity.z * 1E2; // convert m/s to cm/s int16_t nsVog = gps_velocity.x * 1E2; // convert m/s to cm/s int16_t ewVog = gps_velocity.y * 1E2; // convert m/s to cm/s uint8_t fixType = gps.status(); // this lines up perfectly with our enum uint8_t emStatus = 0; // TODO: implement this ENUM. no emergency = 0 uint8_t numSats = gps.num_sats(); uint16_t squawk = 1200; // Mode A code (typically 1200 [0x04B0] for VFR) uint32_t accHoriz = UINT_MAX; float accHoriz_f; if (gps.horizontal_accuracy(accHoriz_f)) { accHoriz = accHoriz_f * 1E3; // convert m to mm } uint16_t accVert = USHRT_MAX; float accVert_f; if (gps.vertical_accuracy(accVert_f)) { accVert = accVert_f * 1E2; // convert m to cm } uint16_t accVel = USHRT_MAX; float accVel_f; if (gps.speed_accuracy(accVel_f)) { accVel = accVel_f * 1E3; // convert m/s to mm/s } uint16_t state = 0; if (out_state._is_in_auto_mode) { state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_AUTOPILOT_ENABLED; } if (!out_state.is_flying) { state |= UAVIONIX_ADSB_OUT_DYNAMIC_STATE_ON_GROUND; } // -------------- // Not Sure uint32_t utcTime = UINT_MAX; // uint32_t utcTime, // TODO: confirm this sets utcTime correctly const uint64_t gps_time = gps.time_epoch_usec(); utcTime = gps_time / 1000000ULL; // -------------- // Unknowns // TODO: implement http://www.srh.noaa.gov/images/epz/wxcalc/pressureAltitude.pdf int32_t altPres = INT_MAX; //_ahrs.get_baro().get_altitude() relative to home, not MSL mavlink_msg_uavionix_adsb_out_dynamic_send( chan, utcTime, latitude, longitude, altGNSS, fixType, numSats, altPres, accHoriz, accVert, accVel, velVert, nsVog, ewVog, emStatus, state, squawk); }