Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: nav-controller-output message sourced from flight mode

Browse Source 
future change will allow overriding these values so each mode can provide the best value
This commit is contained in:
Randy Mackay 2018-12-11 23:04:50 +09:00
parent 2b97e496ff
commit b405286262
3 changed files with 40 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
APMrover2/GCS_Mavlink.cpp
View File

@ -107,12 +107,12 @@ void Rover::send_nav_controller_output(mavlink_channel_t chan)
chan,
0, // roll
degrees(g2.attitude_control.get_desired_pitch()),
nav_controller->nav_bearing_cd() * 0.01f,
nav_controller->target_bearing_cd() * 0.01f,
control_mode->nav_bearing(),
control_mode->wp_bearing(),
MIN(control_mode->get_distance_to_destination(), UINT16_MAX),
0,
control_mode->speed_error(),
nav_controller->crosstrack_error());
control_mode->crosstrack_error());
}
void Rover::send_servo_out(mavlink_channel_t chan)
@ -329,10 +329,8 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
break;
case MSG_NAV_CONTROLLER_OUTPUT:
if (rover.control_mode->is_autopilot_mode()) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
rover.send_nav_controller_output(chan);
}
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
rover.send_nav_controller_output(chan);
break;
case MSG_SERVO_OUT:

33
APMrover2/mode.cpp
View File

@ -163,6 +163,33 @@ void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_
speed_out = throttle * calc_speed_max(g.speed_cruise, g.throttle_cruise * 0.01f);
}
// return heading (in degrees) to target destination (aka waypoint)
float Mode::wp_bearing() const
{
if (!is_autopilot_mode()) {
return 0.0f;
}
return rover.nav_controller->target_bearing_cd() * 0.01f;
}
// return short-term target heading in degrees (i.e. target heading back to line between waypoints)
float Mode::nav_bearing() const
{
if (!is_autopilot_mode()) {
return 0.0f;
}
return rover.nav_controller->nav_bearing_cd() * 0.01f;
}
// return cross track error (i.e. vehicle's distance from the line between waypoints)
float Mode::crosstrack_error() const
{
if (!is_autopilot_mode()) {
return 0.0f;
}
return rover.nav_controller->crosstrack_error();
}
// set desired location
void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{
@ -406,9 +433,9 @@ float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed)
// calculate distance from vehicle to line + wp_overshoot
const float line_yaw_diff = wrap_180_cd(get_bearing_cd(_origin, _destination) - heading_cd);
const float crosstrack_error = rover.nav_controller->crosstrack_error();
const float dist_from_line = fabsf(crosstrack_error);
const bool heading_away = is_positive(line_yaw_diff) == is_positive(crosstrack_error);
const float xtrack_error = rover.nav_controller->crosstrack_error();
const float dist_from_line = fabsf(xtrack_error);
const bool heading_away = is_positive(line_yaw_diff) == is_positive(xtrack_error);
const float wp_overshoot_adj = heading_away ? -dist_from_line : dist_from_line;
// calculate radius of circle that touches vehicle's current position and heading and target position and heading

5
APMrover2/mode.h
View File

@ -79,6 +79,11 @@ public:
virtual bool requires_position() const { return true; }
virtual bool requires_velocity() const { return true; }
// return heading (in degrees) and cross track error (in meteres) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
float wp_bearing() const;
float nav_bearing() const;
float crosstrack_error() const;
//
// navigation methods
//