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

Plane: move sending of GLOBAL_POSITION_INT up to GCS_MAVLINK

Browse Source
This commit is contained in:
Peter Barker 2018-05-01 22:05:26 +10:00 committed by Francisco Ferreira
parent 33582107b6
commit 5aefb90e61
3 changed files with 6 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
ArduPlane/GCS_Mavlink.cpp
View File

@ -178,33 +178,6 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
0, 0, 0, 0);
}
void Plane::send_location(mavlink_channel_t chan)
{
uint32_t fix_time_ms;
// if we have a GPS fix, take the time as the last fix time. That
// allows us to correctly calculate velocities and extrapolate
// 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_ms = gps.last_fix_time_ms();
} else {
fix_time_ms = millis();
}
const Vector3f &vel = gps.velocity();
mavlink_msg_global_position_int_send(
chan,
fix_time_ms,
current_loc.lat, // in 1E7 degrees
current_loc.lng, // in 1E7 degrees
current_loc.alt * 10UL, // millimeters above sea level
relative_altitude * 1.0e3f, // millimeters above ground
vel.x * 100, // X speed cm/s (+ve North)
vel.y * 100, // Y speed cm/s (+ve East)
vel.z * 100, // Z speed cm/s (+ve Down)
ahrs.yaw_sensor);
}
void Plane::send_nav_controller_output(mavlink_channel_t chan)
{
mavlink_msg_nav_controller_output_send(
@ -425,11 +398,6 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
send_power_status();
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
plane.send_location(chan);
break;
case MSG_NAV_CONTROLLER_OUTPUT:
if (plane.control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
@ -1813,3 +1781,8 @@ const AP_FWVersion &GCS_MAVLINK_Plane::get_fwver() const
{
return plane.fwver;
}
int32_t GCS_MAVLINK_Plane::global_position_int_relative_alt() const
{
return plane.relative_altitude * 1.0e3f;
}

1
ArduPlane/GCS_Mavlink.h
View File

@ -34,6 +34,7 @@ protected:
virtual bool in_hil_mode() const override;
int32_t global_position_int_relative_alt() const;
void send_attitude() const override;
private:

1
ArduPlane/Plane.h
View File

@ -771,7 +771,6 @@ private:
void send_fence_status(mavlink_channel_t chan);
void update_sensor_status_flags(void);
void send_extended_status1(mavlink_channel_t chan);
void send_location(mavlink_channel_t chan);
void send_nav_controller_output(mavlink_channel_t chan);
void send_position_target_global_int(mavlink_channel_t chan);
void send_servo_out(mavlink_channel_t chan);