Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 09:43:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

GPS: MAV driver gets support for HIL_GPS message

Browse Source
This commit is contained in:
Randy Mackay 2016-12-07 09:53:28 +09:00
parent 9fc0bc19e7
commit 9e62f12dd7
1 changed files with 99 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
libraries/AP_GPS/AP_GPS_MAV.cpp
View File

@ -42,6 +42,9 @@ bool AP_GPS_MAV::read(void)
// corresponding gps data appropriately; // corresponding gps data appropriately;
void AP_GPS_MAV::handle_msg(const mavlink_message_t *msg) void AP_GPS_MAV::handle_msg(const mavlink_message_t *msg)
{ {
switch (msg->msgid) {
case MAVLINK_MSG_ID_GPS_INPUT: {
mavlink_gps_input_t packet; mavlink_gps_input_t packet;
mavlink_msg_gps_input_decode(msg, &packet); mavlink_msg_gps_input_decode(msg, &packet);
@ -101,8 +104,45 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t *msg)
} }
state.num_sats = packet.satellites_visible; state.num_sats = packet.satellites_visible;
state.last_gps_time_ms = AP_HAL::millis(); state.last_gps_time_ms = AP_HAL::millis();
_new_data = true; _new_data = true;
break;
}
case MAVLINK_MSG_ID_HIL_GPS: {
mavlink_hil_gps_t packet;
mavlink_msg_hil_gps_decode(msg, &packet);
state.time_week = 0;
state.time_week_ms = packet.time_usec/1000;
state.status = (AP_GPS::GPS_Status)packet.fix_type;
Location loc = {};
loc.lat = packet.lat;
loc.lng = packet.lon;
loc.alt = packet.alt;
state.location = loc;
state.location.options = 0;
state.hdop = MAX(packet.eph, 9999);
state.vdop = MAX(packet.epv, 9999);
if (packet.vel < 65535) {
state.ground_speed = packet.vel / 100.0f;
}
Vector3f vel(packet.vn/100.0f, packet.ve/100.0f, packet.vd/100.0f);
state.velocity = vel;
if (packet.cog < 65535) {
state.ground_course = packet.cog / 100.0f;
}
state.have_speed_accuracy = 0;
state.have_horizontal_accuracy = 0;
state.have_vertical_accuracy = 0;
state.num_sats = packet.satellites_visible;
state.last_gps_time_ms = AP_HAL::millis();
_new_data = true;
break;
}
default:
// ignore all other messages
break;
}
} }