Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: move sending of GLOBAL_POSITION_INT up to GCS_MAVLINK

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

36
ArduSub/GCS_Mavlink.cpp
View File

@ -244,24 +244,6 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
} }
void NOINLINE Sub::send_location(mavlink_channel_t chan)
{
const uint32_t now = AP_HAL::millis();
Vector3f vel;
ahrs.get_velocity_NED(vel);
mavlink_msg_global_position_int_send(
chan,
now,
current_loc.lat, // in 1E7 degrees
current_loc.lng, // in 1E7 degrees
ap.depth_sensor_present ? (ahrs.get_home().alt + current_loc.alt) * 10UL : 0, // millimeters above sea level
ap.depth_sensor_present ? current_loc.alt * 10 : 0, // 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); // compass heading in 1/100 degree
}
void NOINLINE Sub::send_nav_controller_output(mavlink_channel_t chan) void NOINLINE Sub::send_nav_controller_output(mavlink_channel_t chan)
{ {
const Vector3f &targets = attitude_control.get_att_target_euler_cd(); const Vector3f &targets = attitude_control.get_att_target_euler_cd();
@ -480,11 +462,6 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
} }
break; break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
sub.send_location(chan);
break;
case MSG_NAV_CONTROLLER_OUTPUT: case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
sub.send_nav_controller_output(chan); sub.send_nav_controller_output(chan);
@ -1487,5 +1464,18 @@ const AP_FWVersion &GCS_MAVLINK_Sub::get_fwver() const
return sub.fwver; return sub.fwver;
} }
int32_t GCS_MAVLINK_Sub::global_position_int_alt() const {
if (!sub.ap.depth_sensor_present) {
return 0;
}
return GCS_MAVLINK::global_position_int_alt();
}
int32_t GCS_MAVLINK_Sub::global_position_int_relative_alt() const {
if (!sub.ap.depth_sensor_present) {
return 0;
}
return GCS_MAVLINK::global_position_int_relative_alt();
}
// dummy method to avoid linking AFS // dummy method to avoid linking AFS
bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) { return false; } bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) { return false; }

3
ArduSub/GCS_Mavlink.h
View File

@ -30,6 +30,9 @@ protected:
// override sending of scaled_pressure3 to send on-board temperature: // override sending of scaled_pressure3 to send on-board temperature:
void send_scaled_pressure3() override; void send_scaled_pressure3() override;
int32_t global_position_int_alt() const override;
int32_t global_position_int_relative_alt() const override;
private: private:
void handleMessage(mavlink_message_t * msg) override; void handleMessage(mavlink_message_t * msg) override;

1
ArduSub/Sub.h
View File

@ -476,7 +476,6 @@ private:
void send_heartbeat(mavlink_channel_t chan); void send_heartbeat(mavlink_channel_t chan);
void send_limits_status(mavlink_channel_t chan); void send_limits_status(mavlink_channel_t chan);
void send_extended_status1(mavlink_channel_t chan); 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_nav_controller_output(mavlink_channel_t chan);
void send_simstate(mavlink_channel_t chan); void send_simstate(mavlink_channel_t chan);
void send_radio_out(mavlink_channel_t chan); void send_radio_out(mavlink_channel_t chan);