Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: move try_send_message of nav_controller_output up

This commit is contained in:
Peter Barker 2018-05-05 10:09:34 +10:00 committed by Andrew Tridgell
parent 57e5991fde
commit d1cada0e25
3 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
APMrover2/GCS_Mavlink.cpp
View File

@ -86,12 +86,18 @@ void GCS_MAVLINK_Rover::get_sensor_status_flags(uint32_t &present,
health = rover.control_sensors_health;
}
void Rover::send_nav_controller_output(mavlink_channel_t chan)
void GCS_MAVLINK_Rover::send_nav_controller_output() const
{
if (!rover.control_mode->is_autopilot_mode()) {
return;
}
const Mode *control_mode = rover.control_mode;
mavlink_msg_nav_controller_output_send(
chan,
0, // roll
degrees(g2.attitude_control.get_desired_pitch()),
degrees(rover.g2.attitude_control.get_desired_pitch()),
control_mode->nav_bearing(),
control_mode->wp_bearing(),
MIN(control_mode->get_distance_to_destination(), UINT16_MAX),
@ -318,11 +324,6 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
switch (id) {
case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
rover.send_nav_controller_output(chan);
break;
case MSG_SERVO_OUT:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
rover.send_servo_out(chan);

2
APMrover2/GCS_Mavlink.h
View File

@ -35,6 +35,8 @@ protected:
bool set_home_to_current_location(bool lock) override;
bool set_home(const Location& loc, bool lock) override;
void send_nav_controller_output() const override;
private:
void handleMessage(mavlink_message_t * msg) override;

1
APMrover2/Rover.h
View File

@ -437,7 +437,6 @@ private:
void fence_check();
// GCS_Mavlink.cpp
void send_nav_controller_output(mavlink_channel_t chan);
void send_servo_out(mavlink_channel_t chan);
void send_pid_tuning(mavlink_channel_t chan);
void send_rpm(mavlink_channel_t chan);