void GCS_MAVLINK_Plane::send_nav_controller_output() const
{
if (plane.control_mode == &plane.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();
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 != &plane.mode_qstabilize) ? quadplane.pos_control->get_alt_error() * 1.0e-2f : 0,
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,
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(plane.auto_state.wp_distance, UINT16_MAX),
plane.altitude_error_cm * 0.01f,
plane.airspeed_error * 100,
nav_controller->crosstrack_error());
}
}
void Rover::send_nav_controller_output(mavlink_channel_t chan)
{
mavlink_msg_nav_controller_output_send(
chan,
g2.attitude_control.get_desired_lat_accel(),
ahrs.groundspeed() * ins.get_gyro().z, // use nav_pitch to hold actual Y accel
nav_controller->nav_bearing_cd() * 0.01f,
nav_controller->target_bearing_cd() * 0.01f,
MIN(control_mode->get_distance_to_destination(), UINT16_MAX),
0,
control_mode->speed_error(),
nav_controller->crosstrack_error());
}
void Tracker::send_nav_controller_output(mavlink_channel_t chan)
{
mavlink_msg_nav_controller_output_send(
chan,
0,
nav_status.pitch,
nav_status.bearing,
nav_status.bearing,
nav_status.distance,
nav_status.altitude_difference,
0,
0);
}
void Rover::send_nav_controller_output(mavlink_channel_t chan)
{
mavlink_msg_nav_controller_output_send(
chan,
lateral_acceleration, // use nav_roll to hold demanded Y accel
ahrs.groundspeed() * ins.get_gyro().z, // use nav_pitch to hold actual Y accel
nav_controller->nav_bearing_cd() * 0.01f,
nav_controller->target_bearing_cd() * 0.01f,
wp_distance,
0,
groundspeed_error,
nav_controller->crosstrack_error());
}
void Rover::send_nav_controller_output(mavlink_channel_t chan)
{
mavlink_msg_nav_controller_output_send(
chan,
0, // roll
degrees(g2.attitude_control.get_desired_pitch()),
control_mode->nav_bearing(),
control_mode->wp_bearing(),
MIN(control_mode->get_distance_to_destination(), UINT16_MAX),
0,
control_mode->speed_error(),
control_mode->crosstrack_error());
}
void NOINLINE Sub::send_nav_controller_output(mavlink_channel_t chan)
{
const Vector3f &targets = attitude_control.get_att_target_euler_cd();
mavlink_msg_nav_controller_output_send(
chan,
targets.x * 1.0e-2f,
targets.y * 1.0e-2f,
targets.z * 1.0e-2f,
wp_nav.get_wp_bearing_to_destination() * 1.0e-2f,
MIN(wp_nav.get_wp_distance_to_destination() * 1.0e-2f, UINT16_MAX),
pos_control.get_alt_error() * 1.0e-2f,
0,
0);
}
void Tracker::send_nav_controller_output(mavlink_channel_t chan)
{
float alt_diff = (g.alt_source == ALT_SOURCE_BARO) ? nav_status.alt_difference_baro : nav_status.alt_difference_gps;
mavlink_msg_nav_controller_output_send(
chan,
0,
nav_status.pitch,
nav_status.bearing,
nav_status.bearing,
MIN(nav_status.distance, UINT16_MAX),
alt_diff,
0,
0);
}
void GCS_MAVLINK_Tracker::send_nav_controller_output() const
{
float alt_diff = (tracker.g.alt_source == ALT_SOURCE_BARO) ? tracker.nav_status.alt_difference_baro : tracker.nav_status.alt_difference_gps;
mavlink_msg_nav_controller_output_send(
chan,
0,
tracker.nav_status.pitch,
tracker.nav_status.bearing,
tracker.nav_status.bearing,
MIN(tracker.nav_status.distance, UINT16_MAX),
alt_diff,
0,
0);
}
void GCS_MAVLINK_Copter::send_nav_controller_output() const
{
if (!copter.ap.initialised) {
return;
}
const Vector3f &targets = copter.attitude_control->get_att_target_euler_cd();
const Copter::Mode *flightmode = copter.flightmode;
mavlink_msg_nav_controller_output_send(
chan,
targets.x * 1.0e-2f,
targets.y * 1.0e-2f,
targets.z * 1.0e-2f,
flightmode->wp_bearing() * 1.0e-2f,
MIN(flightmode->wp_distance() * 1.0e-2f, UINT16_MAX),
copter.pos_control->get_alt_error() * 1.0e-2f,
0,
flightmode->crosstrack_error() * 1.0e-2f);
}
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(rover.g2.attitude_control.get_desired_pitch()),
control_mode->nav_bearing(),
control_mode->wp_bearing(),
MIN(control_mode->get_distance_to_destination(), UINT16_MAX),
0,
control_mode->speed_error(),
control_mode->crosstrack_error());
}
