/*
send the SYSTEM_TIME message
*/
void GCS_MAVLINK::send_system_time(AP_GPS &gps)
{
mavlink_msg_system_time_send(
chan,
gps.time_epoch_usec(),
hal.scheduler->millis());
}
/*
send the SYSTEM_TIME message
*/
void GCS_MAVLINK::send_system_time(AP_GPS &gps)
{
mavlink_msg_system_time_send(
chan,
gps.time_epoch_usec(),
AP_HAL::millis());
}
/*
@brief Generates pseudorandom ICAO from gps time, lat, and lon.
Reference: DO282B, 2.2.4.5.1.3.2
Note gps.time is the number of seconds since UTC midnight
*/
uint32_t AP_ADSB::genICAO(const Location_Class &loc)
{
// gps_time is not seconds since UTC midnight, but it is an equivalent random number
// TODO: use UTC time instead of GPS time
AP_GPS gps = _ahrs.get_gps();
const uint64_t gps_time = gps.time_epoch_usec();
uint32_t timeSum = 0;
uint32_t M3 = 4096 * (loc.lat & 0x00000FFF) + (loc.lng & 0x00000FFF);
for (uint8_t i=0; i<24; i++) {
timeSum += (((gps_time & 0x00FFFFFF)>> i) & 0x00000001);
}
return( (timeSum ^ M3) & 0x00FFFFFF);
}
/*
Send camera feedback to the GCS
*/
void AP_Camera::send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS &ahrs, const Location ¤t_loc)
{
float altitude, altitude_rel;
if (current_loc.flags.relative_alt) {
altitude = current_loc.alt+ahrs.get_home().alt;
altitude_rel = current_loc.alt;
} else {
altitude = current_loc.alt;
altitude_rel = current_loc.alt - ahrs.get_home().alt;
}
mavlink_msg_camera_feedback_send(chan,
gps.time_epoch_usec(),
0, 0, _image_index,
current_loc.lat, current_loc.lng,
altitude/100.0f, altitude_rel/100.0f,
ahrs.roll_sensor/100.0f, ahrs.pitch_sensor/100.0f, ahrs.yaw_sensor/100.0f,
0.0f,CAMERA_FEEDBACK_PHOTO);
}
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);
}
