Browse Source

Plane: Send a quadplane version of NAV_CONTROLLER_OUTPUT

master
Michael du Breuil 7 years ago committed by Andrew Tridgell
parent
commit
3d923d0572
  1. 16
      ArduPlane/GCS_Mavlink.cpp
  2. 6
      ArduPlane/quadplane.cpp
  3. 3
      ArduPlane/quadplane.h

16
ArduPlane/GCS_Mavlink.cpp

@ -180,6 +180,21 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
void Plane::send_nav_controller_output(mavlink_channel_t chan) void Plane::send_nav_controller_output(mavlink_channel_t chan)
{ {
if (quadplane.in_vtol_mode()) {
const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd();
bool wp_nav_valid = quadplane.using_wp_nav();
mavlink_msg_nav_controller_output_send(
chan,
targets.x * 1.0e-2f,
targets.y * 1.0e-2f,
targets.z * 1.0e-2f,
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,
wp_nav_valid ? quadplane.wp_nav->crosstrack_error() : 0);
} else {
mavlink_msg_nav_controller_output_send( mavlink_msg_nav_controller_output_send(
chan, chan,
nav_roll_cd * 0.01f, nav_roll_cd * 0.01f,
@ -191,6 +206,7 @@ void Plane::send_nav_controller_output(mavlink_channel_t chan)
airspeed_error * 100, airspeed_error * 100,
nav_controller->crosstrack_error()); nav_controller->crosstrack_error());
} }
}
void GCS_MAVLINK_Plane::send_position_target_global_int() void GCS_MAVLINK_Plane::send_position_target_global_int()
{ {

6
ArduPlane/quadplane.cpp

@ -2498,6 +2498,12 @@ bool QuadPlane::do_user_takeoff(float takeoff_altitude)
return true; return true;
} }
// return true if the wp_nav controller is being updated
bool QuadPlane::using_wp_nav(void) const
{
return plane.control_mode == QLOITER || plane.control_mode == QLAND || plane.control_mode == QRTL;
}
/* /*
return mav_type for heartbeat return mav_type for heartbeat
*/ */

3
ArduPlane/quadplane.h

@ -110,6 +110,9 @@ public:
// user initiated takeoff for guided mode // user initiated takeoff for guided mode
bool do_user_takeoff(float takeoff_altitude); bool do_user_takeoff(float takeoff_altitude);
// return true if the wp_nav controller is being updated
bool using_wp_nav(void) const;
struct PACKED log_QControl_Tuning { struct PACKED log_QControl_Tuning {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint64_t time_us; uint64_t time_us;

Loading…
Cancel
Save