Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: global-pos-int uses system time

Browse Source 
Also takes vel directly from EKF instead of inertial nav shim
This commit is contained in:
Randy Mackay 2018-02-08 08:13:27 +09:00
parent 24b8ed48b6
commit 81d3a3106e
1 changed files with 7 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduSub/GCS_Mavlink.cpp
View File

@ -243,28 +243,19 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
void NOINLINE Sub::send_location(mavlink_channel_t chan) void NOINLINE Sub::send_location(mavlink_channel_t chan)
{ {
uint32_t fix_time; const uint32_t now = AP_HAL::millis();
// if we have a GPS fix, take the time as the last fix time. That Vector3f vel;
// allows us to correctly calculate velocities and extrapolate ahrs.get_velocity_NED(vel);
// positions.
// If we don't have a GPS fix then we are dead reckoning, and will
// use the current boot time as the fix time.
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
fix_time = gps.last_fix_time_ms();
} else {
fix_time = millis();
}
const Vector3f &vel = inertial_nav.get_velocity();
mavlink_msg_global_position_int_send( mavlink_msg_global_position_int_send(
chan, chan,
fix_time, now,
current_loc.lat, // in 1E7 degrees current_loc.lat, // in 1E7 degrees
current_loc.lng, // in 1E7 degrees current_loc.lng, // in 1E7 degrees
(ahrs.get_home().alt + current_loc.alt) * 10UL, // millimeters above sea level (ahrs.get_home().alt + current_loc.alt) * 10UL, // millimeters above sea level
current_loc.alt * 10, // millimeters above ground current_loc.alt * 10, // millimeters above ground
vel.x, // X speed cm/s (+ve North) vel.x * 100, // X speed cm/s (+ve North)
vel.y, // Y speed cm/s (+ve East) vel.y * 100, // Y speed cm/s (+ve East)
vel.z, // Z speed cm/s (+ve up) vel.z * 100, // Z speed cm/s (+ve Down)
ahrs.yaw_sensor); // compass heading in 1/100 degree ahrs.yaw_sensor); // compass heading in 1/100 degree
} }