aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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

158
libraries/AP_GPS/AP_GPS_MAV.cpp
View File

@ -42,67 +42,107 @@ bool AP_GPS_MAV::read(void)
// corresponding gps data appropriately;
void AP_GPS_MAV::handle_msg(const mavlink_message_t *msg)
{
mavlink_gps_input_t packet;
mavlink_msg_gps_input_decode(msg, &packet);
switch (msg->msgid) {
bool have_alt = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_ALT) == 0);
bool have_hdop = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_HDOP) == 0);
bool have_vdop = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VDOP) == 0);
bool have_vel_h = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VEL_HORIZ) == 0);
bool have_vel_v = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VEL_VERT) == 0);
bool have_sa = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_SPEED_ACCURACY) == 0);
bool have_ha = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_HORIZONTAL_ACCURACY) == 0);
bool have_va = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VERTICAL_ACCURACY) == 0);
case MAVLINK_MSG_ID_GPS_INPUT: {
mavlink_gps_input_t packet;
mavlink_msg_gps_input_decode(msg, &packet);
state.time_week = packet.time_week;
state.time_week_ms = packet.time_week_ms;
state.status = (AP_GPS::GPS_Status)packet.fix_type;
bool have_alt = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_ALT) == 0);
bool have_hdop = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_HDOP) == 0);
bool have_vdop = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VDOP) == 0);
bool have_vel_h = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VEL_HORIZ) == 0);
bool have_vel_v = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VEL_VERT) == 0);
bool have_sa = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_SPEED_ACCURACY) == 0);
bool have_ha = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_HORIZONTAL_ACCURACY) == 0);
bool have_va = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VERTICAL_ACCURACY) == 0);
Location loc = {};
loc.lat = packet.lat;
loc.lng = packet.lon;
if (have_alt) {
loc.alt = packet.alt;
state.time_week = packet.time_week;
state.time_week_ms = packet.time_week_ms;
state.status = (AP_GPS::GPS_Status)packet.fix_type;
Location loc = {};
loc.lat = packet.lat;
loc.lng = packet.lon;
if (have_alt) {
loc.alt = packet.alt;
}
state.location = loc;
state.location.options = 0;
if (have_hdop) {
state.hdop = packet.hdop * 100; // convert to centimeters
}
if (have_vdop) {
state.vdop = packet.vdop * 100; // convert to centimeters
}
if (have_vel_h) {
Vector3f vel(packet.vn, packet.ve, 0);
if (have_vel_v)
vel.z = packet.vd;
state.velocity = vel;
state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x)));
state.ground_speed = norm(vel.x, vel.y);
}
if (have_sa) {
state.speed_accuracy = packet.speed_accuracy;
state.have_speed_accuracy = 1;
}
if (have_ha) {
state.horizontal_accuracy = packet.horiz_accuracy;
state.have_horizontal_accuracy = 1;
}
if (have_va) {
state.vertical_accuracy = packet.vert_accuracy;
state.have_vertical_accuracy = 1;
}
state.num_sats = packet.satellites_visible;
state.last_gps_time_ms = AP_HAL::millis();
_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;
}
state.location = loc;
state.location.options = 0;
if (have_hdop) {
state.hdop = packet.hdop * 100; // convert to centimeters
}
if (have_vdop) {
state.vdop = packet.vdop * 100; // convert to centimeters
}
if (have_vel_h) {
Vector3f vel(packet.vn, packet.ve, 0);
if (have_vel_v)
vel.z = packet.vd;
state.velocity = vel;
state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x)));
state.ground_speed = norm(vel.x, vel.y);
}
if (have_sa) {
state.speed_accuracy = packet.speed_accuracy;
state.have_speed_accuracy = 1;
}
if (have_ha) {
state.horizontal_accuracy = packet.horiz_accuracy;
state.have_horizontal_accuracy = 1;
}
if (have_va) {
state.vertical_accuracy = packet.vert_accuracy;
state.have_vertical_accuracy = 1;
}
state.num_sats = packet.satellites_visible;
state.last_gps_time_ms = AP_HAL::millis();
_new_data = true;
}