aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: move sending of GLOBAL_POSITION_INT up to GCS_MAVLINK

Browse Source
This commit is contained in:
Peter Barker 2018-05-01 22:04:00 +10:00 committed by Francisco Ferreira
parent a81b4266fe
commit 4440f7ec4a
2 changed files with 44 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/GCS_MAVLink/GCS.h
View File

@ -350,6 +350,10 @@ protected:
void send_hwstatus();
void handle_data_packet(mavlink_message_t *msg);
// these two methods are called after current_loc is updated:
virtual int32_t global_position_int_alt() const;
virtual int32_t global_position_int_relative_alt() const;
private:
float adjust_rate_for_stream_trigger(enum streams stream_num);
@ -522,7 +526,12 @@ private:
uint32_t min_sample_counter;
int64_t min_sample_us;
} lag_correction;
// we cache the current location and send it even if the AHRS has
// no idea where we are:
struct Location global_position_current_loc;
void send_global_position_int();
};
/// @class GCS

34
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -2717,6 +2717,35 @@ void GCS_MAVLINK::send_attitude() const
omega.z);
}
int32_t GCS_MAVLINK::global_position_int_alt() const {
return global_position_current_loc.alt * 10UL;
}
int32_t GCS_MAVLINK::global_position_int_relative_alt() const {
const Location &home = AP::ahrs().get_home();
return (global_position_current_loc.alt - home.alt) * 10;
}
void GCS_MAVLINK::send_global_position_int()
{
AP_AHRS &ahrs = AP::ahrs();
ahrs.get_position(global_position_current_loc); // return value ignored; we send stale data
Vector3f vel;
ahrs.get_velocity_NED(vel);
mavlink_msg_global_position_int_send(
chan,
AP_HAL::millis(),
global_position_current_loc.lat, // in 1E7 degrees
global_position_current_loc.lng, // in 1E7 degrees
global_position_int_alt(), // millimeters above ground/sea level
global_position_int_relative_alt(), // millimeters above ground/sea level
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); // compass heading in 1/100 degree
}
bool GCS_MAVLINK::try_send_message(const enum ap_message id)
{
if (telemetry_delayed()) {
@ -2750,6 +2779,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
ret = true;
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_global_position_int();
break;
case MSG_CURRENT_WAYPOINT:
/* fall through */
case MSG_MISSION_ITEM_REACHED: