/**
* Send gps status.
* The GPS status message consists of:
* - satellites visible
* - satellite pnr[] (ignored)
* - satellite used[] (ignored)
* - satellite elevation[] (ignored)
* - satellite azimuth[] (ignored)
* - satellite snr[] (ignored)
*/
static void mavlink_send_gps_status(struct transport_tx *trans, struct link_device *dev)
{
#if USE_GPS
uint8_t nb_sats = Min(GPS_NB_CHANNELS, 20);
uint8_t prn[20];
uint8_t used[20];
uint8_t elevation[20];
uint8_t azimuth[20];
uint8_t snr[20];
for (uint8_t i = 0; i < nb_sats; i++) {
prn[i] = gps.svinfos[i].svid;
// set sat as used if cno > 0, not quite true though
if (gps.svinfos[i].cno > 0) {
used[i] = 1;
}
elevation[i] = gps.svinfos[i].elev;
// convert azimuth to unsigned representation: 0: 0 deg, 255: 360 deg.
uint8_t azim;
if (gps.svinfos[i].azim < 0) {
azim = (float)(-gps.svinfos[i].azim + 180) / 360 * 255;
} else {
azim = (float)gps.svinfos[i].azim / 360 * 255;
}
azimuth[i] = azim;
}
mavlink_msg_gps_status_send(MAVLINK_COMM_0,
gps.num_sv, prn, used, elevation, azimuth, snr);
MAVLinkSendMessage();
#endif
}
void
l_vehicle_gps_position(struct listener *l)
{
struct vehicle_gps_position_s gps;
/* copy gps data into local buffer */
orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
/* GPS position */
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
gps.timestamp,
gps.fix_type,
gps.lat,
gps.lon,
gps.alt,
gps.eph,
gps.epv,
gps.vel,
gps.cog,
gps.satellites_visible);
if (gps.satellite_info_available && (gps_counter % 4 == 0)) {
mavlink_msg_gps_status_send(MAVLINK_COMM_0,
gps.satellites_visible,
gps.satellite_prn,
gps.satellite_used,
gps.satellite_elevation,
gps.satellite_azimuth,
gps.satellite_snr);
}
gps_counter++;
}
void
l_vehicle_gps_position(const struct listener *l)
{
struct vehicle_gps_position_s gps;
/* copy gps data into local buffer */
orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
/* GPS COG is 0..2PI in degrees * 1e2 */
float cog_deg = gps.cog_rad;
if (cog_deg > M_PI_F)
cog_deg -= 2.0f * M_PI_F;
cog_deg *= M_RAD_TO_DEG_F;
/* GPS position */
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
gps.timestamp_position,
gps.fix_type,
gps.lat,
gps.lon,
gps.alt,
cm_uint16_from_m_float(gps.eph_m),
cm_uint16_from_m_float(gps.epv_m),
gps.vel_m_s * 1e2f, // from m/s to cm/s
cog_deg * 1e2f, // from deg to deg * 100
gps.satellites_visible);
/* update SAT info every 10 seconds */
if (gps.satellite_info_available && (gps_counter % 50 == 0)) {
mavlink_msg_gps_status_send(MAVLINK_COMM_0,
gps.satellites_visible,
gps.satellite_prn,
gps.satellite_used,
gps.satellite_elevation,
gps.satellite_azimuth,
gps.satellite_snr);
}
gps_counter++;
}
static void *gps_receiveloop(void * arg) //runs as a pthread and listens messages from GPS
{
while(1)
{
if(0 == global_data_wait(&global_data_gps.access_conf)) //only send if pthread_cond_timedwait received a con signal
{
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, global_data_gps.time_usec, global_data_gps.fix_type, global_data_gps.lat, global_data_gps.lon, global_data_gps.alt, global_data_gps.eph, global_data_gps.epv, global_data_gps.vel, global_data_gps.cog, global_data_gps.satellites_visible);
mavlink_msg_gps_status_send(MAVLINK_COMM_0, global_data_gps.satellites_visible, global_data_gps.satellite_prn, global_data_gps.satellite_used, global_data_gps.satellite_elevation, global_data_gps.satellite_azimuth, global_data_gps.satellite_snr);
}
// else
// {
// mavlink_msg_statustext_send(chan,0,"timeout");
// }
global_data_unlock(&global_data_gps.access_conf);
}
}
