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

Plane: move try_send_message of nav_controller_output up

This commit is contained in:
Peter Barker 2018-05-05 10:10:31 +10:00 committed by Andrew Tridgell
parent 9bc23d3f28
commit 5690a0ea4a
3 changed files with 14 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduPlane/GCS_Mavlink.cpp
View File

@ -164,8 +164,12 @@ void GCS_MAVLINK_Plane::get_sensor_status_flags(uint32_t &present,
health = plane.control_sensors_health;
}
void Plane::send_nav_controller_output(mavlink_channel_t chan)
void GCS_MAVLINK_Plane::send_nav_controller_output() const
{
if (plane.control_mode == MANUAL) {
return;
}
const QuadPlane &quadplane = plane.quadplane;
if (quadplane.in_vtol_mode()) {
const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd();
bool wp_nav_valid = quadplane.using_wp_nav();
@ -178,18 +182,19 @@ void Plane::send_nav_controller_output(mavlink_channel_t chan)
wp_nav_valid ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0,
wp_nav_valid ? MIN(quadplane.wp_nav->get_wp_distance_to_destination(), UINT16_MAX) : 0,
(plane.control_mode != QSTABILIZE) ? quadplane.pos_control->get_alt_error() * 1.0e-2f : 0,
airspeed_error * 100,
plane.airspeed_error * 100,
wp_nav_valid ? quadplane.wp_nav->crosstrack_error() : 0);
} else {
const AP_Navigation *nav_controller = plane.nav_controller;
mavlink_msg_nav_controller_output_send(
chan,
nav_roll_cd * 0.01f,
nav_pitch_cd * 0.01f,
plane.nav_roll_cd * 0.01f,
plane.nav_pitch_cd * 0.01f,
nav_controller->nav_bearing_cd() * 0.01f,
nav_controller->target_bearing_cd() * 0.01f,
MIN(auto_state.wp_distance, UINT16_MAX),
altitude_error_cm * 0.01f,
airspeed_error * 100,
MIN(plane.auto_state.wp_distance, UINT16_MAX),
plane.altitude_error_cm * 0.01f,
plane.airspeed_error * 100,
nav_controller->crosstrack_error());
}
}
@ -420,13 +425,6 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
switch (id) {
case MSG_NAV_CONTROLLER_OUTPUT:
if (plane.control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
plane.send_nav_controller_output(chan);
}
break;
case MSG_SERVO_OUT:
#if HIL_SUPPORT
if (plane.g.hil_mode == 1) {

2
ArduPlane/GCS_Mavlink.h
View File

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

1
ArduPlane/Plane.h
View File

@ -799,7 +799,6 @@ private:
void update_load_factor(void);
void send_fence_status(mavlink_channel_t chan);
void update_sensor_status_flags(void);
void send_nav_controller_output(mavlink_channel_t chan);
void send_servo_out(mavlink_channel_t chan);
void send_wind(mavlink_channel_t chan);
void send_pid_info(const mavlink_channel_t chan, const AP_Logger::PID_Info *pid_info, const uint8_t axis, const float achieved);