aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: integrate navigation library

Browse Source
This commit is contained in:
Randy Mackay 2019-04-29 15:31:45 +09:00
parent c3948bb074
commit a94ebc5bc3
15 changed files with 244 additions and 335 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
APMrover2/APMrover2.cpp
View File

@ -291,6 +291,9 @@ void Rover::one_second_loop(void)
// need to set "likely flying" when armed to allow for compass
// learning to run
ahrs.set_likely_flying(hal.util->get_soft_armed());
// send latest param values to wp_nav
g2.wp_nav.set_turn_params(g.turn_max_g, g2.turn_radius, g2.motors.have_skid_steering());
}
void Rover::update_GPS(void)

6
APMrover2/Log.cpp
View File

@ -130,10 +130,10 @@ void Rover::Log_Write_Nav_Tuning()
LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
time_us : AP_HAL::micros64(),
wp_distance : control_mode->get_distance_to_destination(),
wp_bearing_cd : (uint16_t)wrap_360_cd(nav_controller->target_bearing_cd()),
nav_bearing_cd : (uint16_t)wrap_360_cd(nav_controller->nav_bearing_cd()),
wp_bearing_cd : (uint16_t)wrap_360_cd(control_mode->wp_bearing() * 100),
nav_bearing_cd : (uint16_t)wrap_360_cd(control_mode->nav_bearing() * 100),
yaw : (uint16_t)ahrs.yaw_sensor,
xtrack_error : nav_controller->crosstrack_error()
xtrack_error : control_mode->crosstrack_error()
};
logger.WriteBlock(&pkt, sizeof(pkt));
}

110
APMrover2/Parameters.cpp
View File

@ -93,14 +93,6 @@ const AP_Param::Info Rover::var_info[] = {
// @User: Standard
GSCALAR(speed_cruise, "CRUISE_SPEED", CRUISE_SPEED),
// @Param: PIVOT_TURN_ANGLE
// @DisplayName: Pivot turn angle
// @Description: Navigation angle threshold in degrees to switch to pivot steering when SKID_STEER_OUT is 1. This allows you to setup a skid steering rover to turn on the spot in auto mode when the angle it needs to turn it greater than this angle. An angle of zero means to disable pivot turning. Note that you will probably also want to set a low value for WP_RADIUS to get neat turns.
// @Units: deg
// @Range: 0 360
// @Increment: 1
// @User: Standard
GSCALAR(pivot_turn_angle, "PIVOT_TURN_ANGLE", 60),
// @Param: CRUISE_THROTTLE
// @DisplayName: Base throttle percentage in auto
@ -260,24 +252,6 @@ const AP_Param::Info Rover::var_info[] = {
// @User: Standard
GSCALAR(mode6, "MODE6", Mode::Number::MANUAL),
// @Param: WP_RADIUS
// @DisplayName: Waypoint radius
// @Description: The distance in meters from a waypoint when we consider the waypoint has been reached. This determines when the rover will turn along the next waypoint path.
// @Units: m
// @Range: 0 1000
// @Increment: 0.1
// @User: Standard
GSCALAR(waypoint_radius, "WP_RADIUS", 2.0f),
// @Param: WP_OVERSHOOT
// @DisplayName: Waypoint overshoot maximum
// @Description: Waypoint overshoot maximum in meters. The vehicle will attempt to stay within this many meters of the track as it completes one waypoint and moves to the next.
// @Units: m
// @Range: 0 10
// @Increment: 0.1
// @User: Standard
GSCALAR(waypoint_overshoot, "WP_OVERSHOOT", 2.0f),
// @Param: TURN_MAX_G
// @DisplayName: Turning maximum G force
// @Description: The maximum turning acceleration (in units of gravities) that the rover can handle while remaining stable. The navigation code will keep the lateral acceleration below this level to avoid rolling over or slipping the wheels in turns
@ -507,14 +481,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp
AP_SUBGROUPINFO(smart_rtl, "SRTL_", 13, ParametersG2, AP_SmartRTL),
// @Param: WP_SPEED
// @DisplayName: Waypoint speed default
// @Description: Waypoint speed default. If zero use CRUISE_SPEED.
// @Units: m/s
// @Range: 0 100
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("WP_SPEED", 14, ParametersG2, wp_speed, 0.0f),
// 14 was WP_SPEED and should not be re-used
// @Param: RTL_SPEED
// @DisplayName: Return-to-Launch speed default
@ -544,14 +511,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp
AP_SUBGROUPINFO(avoid, "AVOID_", 19, ParametersG2, AC_Avoid),
// @Param: PIVOT_TURN_RATE
// @DisplayName: Pivot turn rate
// @Description: Desired pivot turn rate in deg/s.
// @Units: deg/s
// @Range: 0 360
// @Increment: 1
// @User: Standard
AP_GROUPINFO("PIVOT_TURN_RATE", 20, ParametersG2, pivot_turn_rate, 90),
// 20 was PIVOT_TURN_RATE and should not be re-used
// @Param: BAL_PITCH_MAX
// @DisplayName: BalanceBot Maximum Pitch
@ -708,6 +668,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Advanced
AP_GROUPINFO("STICK_MIXING", 42, ParametersG2, stick_mixing, 0),
// @Group: WP_
// @Path: ../libraries/AR_WPNav/AR_WPNav.cpp
AP_SUBGROUPINFO(wp_nav, "WP_", 43, ParametersG2, AR_WPNav),
AP_GROUPEND
};
@ -725,6 +689,46 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Description: RC Channel to use for auxiliary functions including saving waypoints
// @User: Advanced
// @Param: PIVOT_TURN_ANGLE
// @DisplayName: Pivot turn angle
// @Description: Navigation angle threshold in degrees to switch to pivot steering when SKID_STEER_OUT is 1. This allows you to setup a skid steering rover to turn on the spot in auto mode when the angle it needs to turn it greater than this angle. An angle of zero means to disable pivot turning. Note that you will probably also want to set a low value for WP_RADIUS to get neat turns.
// @Units: deg
// @Range: 0 360
// @Increment: 1
// @User: Standard
// @Param: WP_RADIUS
// @DisplayName: Waypoint radius
// @Description: The distance in meters from a waypoint when we consider the waypoint has been reached. This determines when the rover will turn along the next waypoint path.
// @Units: m
// @Range: 0 1000
// @Increment: 0.1
// @User: Standard
// @Param: WP_OVERSHOOT
// @DisplayName: Waypoint overshoot maximum
// @Description: Waypoint overshoot maximum in meters. The vehicle will attempt to stay within this many meters of the track as it completes one waypoint and moves to the next.
// @Units: m
// @Range: 0 10
// @Increment: 0.1
// @User: Standard
// @Param: WP_SPEED
// @DisplayName: Waypoint speed default
// @Description: Waypoint speed default. If zero use CRUISE_SPEED.
// @Units: m/s
// @Range: 0 100
// @Increment: 0.1
// @User: Standard
// @Param: PIVOT_TURN_RATE
// @DisplayName: Pivot turn rate
// @Description: Desired pivot turn rate in deg/s.
// @Units: deg/s
// @Range: 0 360
// @Increment: 1
// @User: Standard
ParametersG2::ParametersG2(void)
:
#if ADVANCED_FAILSAFE == ENABLED
@ -739,7 +743,8 @@ ParametersG2::ParametersG2(void)
avoid(rover.ahrs, fence, rover.g2.proximity, &rover.g2.beacon),
follow(),
windvane(),
airspeed()
airspeed(),
wp_nav(attitude_control, rover.L1_controller)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -772,6 +777,11 @@ const AP_Param::ConversionInfo conversion_table[] = {
{ Parameters::k_param_throttle_max_old, 0, AP_PARAM_INT8, "MOT_THR_MAX" },
{ Parameters::k_param_compass_enabled_deprecated, 0, AP_PARAM_INT8, "COMPASS_ENABLE" },
{ Parameters::k_param_pivot_turn_angle, 0, AP_PARAM_INT16, "WP_PIVOT_ANGLE" },
{ Parameters::k_param_waypoint_radius, 0, AP_PARAM_FLOAT, "WP_RADIUS" },
{ Parameters::k_param_waypoint_overshoot, 0, AP_PARAM_FLOAT, "WP_OVERSHOOT" },
{ Parameters::k_param_g2, 20, AP_PARAM_INT16, "WP_PIVOT_RATE" },
};
void Rover::load_parameters(void)
@ -812,7 +822,7 @@ void Rover::load_parameters(void)
g2.crash_angle.set_default(0);
g2.loit_type.set_default(1);
g2.loit_radius.set_default(5);
g.waypoint_overshoot.set_default(10);
g2.wp_nav.set_default_overshoot(10);
}
const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old, Parameters::k_param_rc_2_old,
@ -840,6 +850,18 @@ void Rover::load_parameters(void)
}
}
// set AR_WPNav's WP_SPEED to be old WP_SPEED (if set) or CRUISE_SPEED (if set)
const AP_Param::ConversionInfo wp_speed_old_info = { Parameters::k_param_g2, 14, AP_PARAM_FLOAT, "WP_SPEED" };
const AP_Param::ConversionInfo cruise_speed_info = { Parameters::k_param_speed_cruise, 0, AP_PARAM_FLOAT, "WP_SPEED" };
AP_Float wp_speed_old;
if (AP_Param::find_old_parameter(&wp_speed_old_info, &wp_speed_old)) {
// old WP_SPEED parameter value was set so copy to new WP_SPEED
AP_Param::convert_old_parameter(&wp_speed_old_info, 1.0f);
} else {
// copy CRUISE_SPEED to new WP_SPEED
AP_Param::convert_old_parameter(&cruise_speed_info, 1.0f);
}
// configure safety switch to allow stopping the motors while armed
#if HAL_HAVE_SAFETY_SWITCH
AP_Param::set_default_by_name("BRD_SAFETYOPTION", AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF|

20
APMrover2/Parameters.h
View File

@ -36,7 +36,7 @@ public:
k_param_scheduler,
k_param_relay,
k_param_BoardConfig,
k_param_pivot_turn_angle,
k_param_pivot_turn_angle, // unused
k_param_rc_13_old, // unused
k_param_rc_14_old, // unused
@ -173,8 +173,8 @@ public:
//
k_param_command_total = 220, // unused
k_param_command_index, // unused
k_param_waypoint_radius,
k_param_waypoint_overshoot,
k_param_waypoint_radius, // unused
k_param_waypoint_overshoot, // unused
//
// camera control
@ -234,7 +234,6 @@ public:
AP_Int8 auto_trigger_pin;
AP_Float auto_kickstart;
AP_Float turn_max_g;
AP_Int16 pivot_turn_angle;
AP_Int16 gcs_pid_mask;
// Throttle
@ -269,11 +268,6 @@ public:
AP_Int8 mode5;
AP_Int8 mode6;
// Waypoints
//
AP_Float waypoint_radius;
AP_Float waypoint_overshoot;
Parameters() {}
};
@ -330,8 +324,7 @@ public:
// Safe RTL library
AP_SmartRTL smart_rtl;
// default speeds for auto, rtl
AP_Float wp_speed;
// default speeds for rtl
AP_Float rtl_speed;
// frame class for vehicle
@ -346,9 +339,6 @@ public:
// avoidance library
AC_Avoid avoid;
// pivot turn rate
AP_Int16 pivot_turn_rate;
// pitch angle at 100% throttle
AP_Float bal_pitch_max;
@ -404,6 +394,8 @@ public:
AP_Scripting scripting;
#endif // ENABLE_SCRIPTING
// waypoint navigation
AR_WPNav wp_nav;
};
extern const AP_Param::Info var_info[];

1
APMrover2/Rover.cpp
View File

@ -29,7 +29,6 @@ Rover::Rover(void) :
channel_lateral(nullptr),
logger{g.log_bitmask},
modes(&g.mode1),
nav_controller(&L1_controller),
control_mode(&mode_initializing),
G_Dt(0.02f)
{

9
APMrover2/Rover.h
View File

@ -65,6 +65,7 @@
#include <AP_WheelEncoder/AP_WheelEncoder.h>
#include <AP_WheelEncoder/AP_WheelRateControl.h>
#include <APM_Control/AR_AttitudeControl.h>
#include <AR_WPNav/AR_WPNav.h>
#include <AP_SmartRTL/AP_SmartRTL.h>
#include <AP_Logger/AP_Logger.h>
#include <Filter/AverageFilter.h> // Mode Filter from Filter library
@ -198,9 +199,6 @@ private:
AP_L1_Control L1_controller{ahrs, nullptr};
// selected navigation controller
AP_Navigation *nav_controller;
#if AP_AHRS_NAVEKF_AVAILABLE
OpticalFlow optflow;
#endif
@ -307,9 +305,6 @@ private:
// flyforward timer
uint32_t flyforward_start_ms;
// true if pivoting (set by use_pivot_steering)
bool pivot_steering_active;
static const AP_Scheduler::Task scheduler_tasks[];
static const AP_Param::Info var_info[];
@ -474,8 +469,6 @@ private:
void rpm_update(void);
// Steering.cpp
bool use_pivot_steering_at_next_WP(float yaw_error_cd);
bool use_pivot_steering(float yaw_error_cd);
void set_servos(void);
// system.cpp

48
APMrover2/Steering.cpp
View File

@ -1,53 +1,5 @@
#include "Rover.h"
/*
work out if we are going to use pivot steering at next waypoint
*/
bool Rover::use_pivot_steering_at_next_WP(float yaw_error_cd)
{
// check cases where we clearly cannot use pivot steering
if (!g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) {
return false;
}
// if error is larger than pivot_turn_angle then use pivot steering at next WP
if (fabsf(yaw_error_cd) * 0.01f > g.pivot_turn_angle) {
return true;
}
return false;
}
/*
work out if we are going to use pivot steering
*/
bool Rover::use_pivot_steering(float yaw_error_cd)
{
// check cases where we clearly cannot use pivot steering
if (!g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) {
pivot_steering_active = false;
return false;
}
// calc bearing error
const float yaw_error = fabsf(yaw_error_cd) * 0.01f;
// if error is larger than pivot_turn_angle start pivot steering
if (yaw_error > g.pivot_turn_angle) {
pivot_steering_active = true;
return true;
}
// if within 10 degrees of the target heading, exit pivot steering
if (yaw_error < 10.0f) {
pivot_steering_active = false;
return false;
}
// by default stay in
return pivot_steering_active;
}
/*****************************************
Set the flight control servos based on the current calculated values
*****************************************/

165
APMrover2/mode.cpp
View File

@ -165,7 +165,7 @@ float Mode::wp_bearing() const
if (!is_autopilot_mode()) {
return 0.0f;
}
return rover.nav_controller->target_bearing_cd() * 0.01f;
return g2.wp_nav.wp_bearing_cd() * 0.01f;
}
// return short-term target heading in degrees (i.e. target heading back to line between waypoints)
@ -174,7 +174,7 @@ float Mode::nav_bearing() const
if (!is_autopilot_mode()) {
return 0.0f;
}
return rover.nav_controller->nav_bearing_cd() * 0.01f;
return g2.wp_nav.nav_bearing_cd() * 0.01f;
}
// return cross track error (i.e. vehicle's distance from the line between waypoints)
@ -183,57 +183,17 @@ float Mode::crosstrack_error() const
if (!is_autopilot_mode()) {
return 0.0f;
}
return rover.nav_controller->crosstrack_error();
return g2.wp_nav.crosstrack_error();
}
// set desired location
void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{
// set origin to last destination if waypoint controller active
if ((AP_HAL::millis() - last_steer_to_wp_ms < 100) && _reached_destination) {
_origin = _destination;
} else {
// otherwise use reasonable stopping point
calc_stopping_location(_origin);
}
_destination = destination;
g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd);
// initialise distance
_distance_to_destination = _origin.get_distance(_destination);
_distance_to_destination = g2.wp_nav.get_distance_to_destination();
_reached_destination = false;
// set final desired speed
_desired_speed_final = 0.0f;
if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {
const float curr_leg_bearing_cd = _origin.get_bearing_to(_destination);
const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
if (fabsf(turn_angle_cd) < 10.0f) {
// if turning less than 0.1 degrees vehicle can continue at full speed
// we use 0.1 degrees instead of zero to avoid divide by zero in calcs below
_desired_speed_final = _desired_speed;
} else if (rover.use_pivot_steering_at_next_WP(turn_angle_cd)) {
// pivoting so we will stop
_desired_speed_final = 0.0f;
} else {
// calculate maximum speed that keeps overshoot within bounds
const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
_desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m));
}
}
}
// set desired location as an offset from the EKF origin in NED frame
bool Mode::set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd)
{
Location destination_ned;
// initialise destination to ekf origin
if (!ahrs.get_origin(destination_ned)) {
return false;
}
// apply offset
destination_ned.offset(destination.x, destination.y);
set_desired_location(destination_ned, next_leg_bearing_cd);
return true;
}
// set desired heading and speed
@ -247,19 +207,17 @@ void Mode::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
_desired_speed = target_speed;
}
// get default speed for this mode (held in (CRUISE_SPEED, WP_SPEED or RTL_SPEED)
// get default speed for this mode (held in (WP_SPEED or RTL_SPEED)
float Mode::get_speed_default(bool rtl) const
{
if (rtl && is_positive(g2.rtl_speed)) {
return g2.rtl_speed;
} else if (is_positive(g2.wp_speed)) {
return g2.wp_speed;
} else {
return g.speed_cruise;
}
return g2.wp_nav.get_default_speed();
}
// restore desired speed to default from parameter values (CRUISE_SPEED or WP_SPEED)
// restore desired speed to default from parameter values (WP_SPEED)
void Mode::set_desired_speed_to_default(bool rtl)
{
_desired_speed = get_speed_default(rtl);
@ -278,7 +236,7 @@ bool Mode::set_desired_speed(float speed)
// execute the mission in reverse (i.e. backing up)
void Mode::set_reversed(bool value)
{
_reversed = value;
g2.wp_nav.set_reversed(value);
}
// handle tacking request (from auxiliary switch) in sailboats
@ -415,81 +373,48 @@ float Mode::calc_speed_nudge(float target_speed, float cruise_speed, float cruis
return target_speed + speed_nudge;
}
// calculated a reduced speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint
// should be called after calc_lateral_acceleration and before calc_throttle
// relies on these internal members being updated: _yaw_error_cd, _distance_to_destination
float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed)
// high level call to navigate to waypoint
// uses wp_nav to calculate turn rate and speed to drive along the path from origin to destination
// this function updates _distance_to_destination and _yaw_error_cd
void Mode::navigate_to_waypoint()
{
// reduce speed to zero during pivot turns
if (rover.use_pivot_steering(_yaw_error_cd)) {
return 0.0f;
// update navigation controller
g2.wp_nav.update(rover.G_Dt);
_distance_to_destination = g2.wp_nav.get_distance_to_destination();
// pass speed to throttle controller
const float desired_speed = g2.wp_nav.get_speed();
calc_throttle(desired_speed, true, true);
float desired_heading_cd = g2.wp_nav.wp_bearing_cd();
_yaw_error_cd = wrap_180_cd(desired_heading_cd - ahrs.yaw_sensor);
if (rover.sailboat_use_indirect_route(desired_heading_cd)) {
// sailboats use heading controller when tacking upwind
desired_heading_cd = rover.sailboat_calc_heading(desired_heading_cd);
calc_steering_to_heading(desired_heading_cd, g2.wp_nav.get_pivot_rate());
} else {
// call turn rate steering controller
calc_steering_from_turn_rate(g2.wp_nav.get_turn_rate_rads(), desired_speed, g2.wp_nav.get_reversed());
}
// reduce speed to limit overshoot from line between origin and destination
// calculate number of degrees vehicle must turn to face waypoint
const float heading_cd = is_negative(desired_speed) ? wrap_180_cd(ahrs.yaw_sensor + 18000) : ahrs.yaw_sensor;
const float wp_yaw_diff = wrap_180_cd(rover.nav_controller->target_bearing_cd() - heading_cd);
const float turn_angle_rad = fabsf(radians(wp_yaw_diff * 0.01f));
// calculate distance from vehicle to line + wp_overshoot
const float line_yaw_diff = wrap_180_cd(_origin.get_bearing_to(_destination) - heading_cd);
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
float radius_m = 999.0f;
float radius_calc_denom = fabsf(1.0f - cosf(turn_angle_rad));
if (!is_zero(radius_calc_denom)) {
radius_m = MAX(0.0f, rover.g.waypoint_overshoot + wp_overshoot_adj) / radius_calc_denom;
}
// calculate and limit speed to allow vehicle to stay on circle
float overshoot_speed_max = safe_sqrt(g.turn_max_g * GRAVITY_MSS * MAX(g2.turn_radius, radius_m));
float speed_max = constrain_float(desired_speed, -overshoot_speed_max, overshoot_speed_max);
// limit speed based on distance to waypoint and max acceleration/deceleration
if (is_positive(_distance_to_destination) && is_positive(attitude_control.get_decel_max())) {
const float dist_speed_max = safe_sqrt(2.0f * _distance_to_destination * attitude_control.get_decel_max() + sq(_desired_speed_final));
speed_max = constrain_float(speed_max, -dist_speed_max, dist_speed_max);
}
// return minimum speed
return speed_max;
}
// calculate the lateral acceleration target to cause the vehicle to drive along the path from origin to destination
// this function updates the _yaw_error_cd value
void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed)
// calculate steering output given a turn rate and speed
void Mode::calc_steering_from_turn_rate(float turn_rate, float speed, bool reversed)
{
// record system time of call
last_steer_to_wp_ms = AP_HAL::millis();
// Calculate the required turn of the wheels
// negative error = left turn
// positive error = right turn
rover.nav_controller->set_reverse(reversed);
rover.nav_controller->update_waypoint(origin, destination, g.waypoint_radius);
float desired_lat_accel = rover.nav_controller->lateral_acceleration();
float desired_heading = rover.nav_controller->target_bearing_cd();
if (reversed) {
desired_heading = wrap_360_cd(desired_heading + 18000);
desired_lat_accel *= -1.0f;
// add obstacle avoidance response to lateral acceleration target
// ToDo: replace this type of object avoidance with path planning
if (!reversed) {
const float lat_accel_adj = (rover.obstacle.turn_angle / 45.0f) * g.turn_max_g;
turn_rate += attitude_control.get_turn_rate_from_lat_accel(lat_accel_adj, speed);
}
_yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor);
if (rover.sailboat_use_indirect_route(desired_heading)) {
// sailboats use heading controller when tacking upwind
desired_heading = rover.sailboat_calc_heading(desired_heading);
calc_steering_to_heading(desired_heading, g2.pivot_turn_rate);
} else if (rover.use_pivot_steering(_yaw_error_cd)) {
// for pivot turns use heading controller
calc_steering_to_heading(desired_heading, g2.pivot_turn_rate);
} else {
// call lateral acceleration to steering controller
calc_steering_from_lateral_acceleration(desired_lat_accel, reversed);
}
// calculate and send final steering command to motor library
const float steering_out = attitude_control.get_steering_out_rate(turn_rate,
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
g2.motors.set_steering(steering_out * 4500.0f);
}
/*

32
APMrover2/mode.h
View File

@ -105,9 +105,6 @@ public:
// next_leg_bearing_cd should be heading to the following waypoint (used to slow the vehicle in order to make the turn)
virtual void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN);
// set desired location as offset from the EKF origin, return true on success
bool set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN);
// true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
virtual bool reached_destination() const { return true; }
@ -156,8 +153,11 @@ protected:
// decode pilot's input and return heading_out (in cd) and speed_out (in m/s)
void get_pilot_desired_heading_and_speed(float &heading_out, float &speed_out);
// calculate steering output to drive along line from origin to destination waypoint
void calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed = false);
// high level call to navigate to waypoint
void navigate_to_waypoint();
// calculate steering output given a turn rate and speed
void calc_steering_from_turn_rate(float turn_rate, float speed, bool reversed);
// calculate steering angle given a desired lateral acceleration
void calc_steering_from_lateral_acceleration(float lat_accel, bool reversed = false);
@ -183,11 +183,6 @@ protected:
// return value is a new speed (in m/s) which up to the projected maximum speed based on the cruise speed and cruise throttle
float calc_speed_nudge(float target_speed, float cruise_speed, float cruise_throttle);
// calculated a reduced speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint
// should be called after calc_steering_to_waypoint and before calc_throttle
// relies on these internal members being updated: lateral_acceleration, _yaw_error_cd, _distance_to_destination
float calc_reduced_speed_for_turn_or_distance(float desired_speed);
// calculate vehicle stopping location using current location, velocity and maximum acceleration
void calc_stopping_location(Location& stopping_loc);
@ -208,19 +203,13 @@ protected:
class RC_Channel *&channel_lateral;
class AR_AttitudeControl &attitude_control;
// private members for waypoint navigation
Location _origin; // origin Location (vehicle will travel from the origin to the destination)
Location _destination; // destination Location when in Guided_WP
float _distance_to_destination; // distance from vehicle to final destination in meters
bool _reached_destination; // true once the vehicle has reached the destination
float _desired_yaw_cd; // desired yaw in centi-degrees
float _yaw_error_cd; // error between desired yaw and actual yaw in centi-degrees
float _desired_speed; // desired speed in m/s
float _desired_speed_final; // desired speed in m/s when we reach the destination
float _speed_error; // ground speed error in m/s
uint32_t last_steer_to_wp_ms; // system time of last call to calc_steering_to_waypoint
bool _reversed; // execute the mission by backing up
};
@ -414,6 +403,7 @@ protected:
bool have_attitude_target; // true if we have a valid attitude target
uint32_t _des_att_time_ms; // system time last call to set_desired_attitude was made (used for timeout)
float _desired_yaw_rate_cds;// target turn rate centi-degrees per second
bool sent_notification; // used to send one time notification to ground station
// limits
struct {
@ -457,8 +447,8 @@ public:
bool is_autopilot_mode() const override { return true; }
// return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
float wp_bearing() const override { return _desired_yaw_cd; }
float nav_bearing() const override { return _desired_yaw_cd; }
float wp_bearing() const override { return _desired_yaw_cd * 0.01f; }
float nav_bearing() const override { return _desired_yaw_cd * 0.01f; }
float crosstrack_error() const override { return 0.0f; }
// return distance (in meters) to destination
@ -467,6 +457,8 @@ public:
protected:
bool _enter() override;
Location _destination; // target location to hold position around
};
class ModeManual : public Mode
@ -507,11 +499,13 @@ public:
bool is_autopilot_mode() const override { return true; }
float get_distance_to_destination() const override { return _distance_to_destination; }
bool reached_destination() const override { return _reached_destination; }
bool reached_destination() const override;
protected:
bool _enter() override;
bool sent_notification; // used to send one time notification to ground station
};
class ModeSmartRTL : public Mode

2
APMrover2/mode_acro.cpp
View File

@ -28,7 +28,7 @@ void ModeAcro::update()
if (g2.motors.has_sail() && rover.sailboat_tacking()) {
// call heading controller during tacking
steering_out = attitude_control.get_steering_out_heading(rover.sailboat_get_tack_heading_rad(),
g2.pivot_turn_rate,
g2.wp_nav.get_pivot_rate(),
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);

43
APMrover2/mode_auto.cpp
View File

@ -12,10 +12,10 @@ bool ModeAuto::_enter()
}
// initialise waypoint speed
set_desired_speed_to_default();
g2.wp_nav.set_desired_speed_to_default();
// init location target
set_desired_location(rover.current_loc);
g2.wp_nav.set_desired_location(rover.current_loc);
// other initialisation
auto_triggered = false;
@ -41,18 +41,9 @@ void ModeAuto::update()
switch (_submode) {
case Auto_WP:
{
_distance_to_destination = rover.current_loc.get_distance(_destination);
const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius;
// check if we've reached the destination
if (!_reached_destination && (near_wp || rover.current_loc.past_interval_finish_line(_origin, _destination))) {
// trigger reached
_reached_destination = true;
}
// determine if we should keep navigating
if (!_reached_destination) {
// continue driving towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
if (!g2.wp_nav.reached_destination()) {
// update navigation controller
navigate_to_waypoint();
} else {
// we have reached the destination so stay here
if (rover.is_boat()) {
@ -62,6 +53,8 @@ void ModeAuto::update()
} else {
stop_vehicle();
}
// update distance to destination
_distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
}
break;
}
@ -118,10 +111,22 @@ void ModeAuto::calc_throttle(float target_speed, bool nudge_allowed, bool avoida
// return distance (in meters) to destination
float ModeAuto::get_distance_to_destination() const
{
if (_submode == Auto_RTL) {
switch (_submode) {
case Auto_WP:
return _distance_to_destination;
case Auto_HeadingAndSpeed:
// no valid distance so return zero
return 0.0f;
case Auto_RTL:
return rover.mode_rtl.get_distance_to_destination();
case Auto_Loiter:
return rover.mode_loiter.get_distance_to_destination();
case Auto_Guided:
return rover.mode_guided.get_distance_to_destination();
}
return _distance_to_destination;
// this line should never be reached
return 0.0f;
}
// set desired location to drive to
@ -138,7 +143,7 @@ bool ModeAuto::reached_destination() const
{
switch (_submode) {
case Auto_WP:
return _reached_destination;
return g2.wp_nav.reached_destination();
break;
case Auto_HeadingAndSpeed:
// always return true because this is the safer option to allow missions to continue
@ -523,7 +528,7 @@ void ModeAuto::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd)
}
// set auto target
const float speed_max = get_speed_default();
const float speed_max = g2.wp_nav.get_default_speed();
set_desired_heading_and_speed(desired_heading_cd, constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max));
}
@ -595,7 +600,7 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
// if a location target was set, return true once vehicle is close
if (guided_target.valid) {
if (rover.current_loc.get_distance(guided_target.loc) <= rover.g.waypoint_radius) {
if (rover.current_loc.get_distance(guided_target.loc) <= rover.g2.wp_nav.get_radius()) {
return true;
}
}

61
APMrover2/mode_guided.cpp
View File

@ -3,12 +3,16 @@
bool ModeGuided::_enter()
{
// initialise waypoint speed
set_desired_speed_to_default();
// set desired location to reasonable stopping point
calc_stopping_location(_destination);
set_desired_location(_destination);
if (!g2.wp_nav.set_desired_location_to_stopping_location()) {
return false;
}
_guided_mode = Guided_WP;
// initialise waypoint speed
g2.wp_nav.set_desired_speed_to_default();
sent_notification = false;
return true;
}
@ -18,19 +22,17 @@ void ModeGuided::update()
switch (_guided_mode) {
case Guided_WP:
{
_distance_to_destination = rover.current_loc.get_distance(_destination);
const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius;
// check if we've reached the destination
if (!_reached_destination && (near_wp || rover.current_loc.past_interval_finish_line(_origin, _destination))) {
_reached_destination = true;
rover.gcs().send_mission_item_reached_message(0);
}
// determine if we should keep navigating
if (!_reached_destination) {
// drive towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
if (!g2.wp_nav.reached_destination()) {
// update navigation controller
navigate_to_waypoint();
} else {
// send notification
if (!sent_notification) {
sent_notification = true;
rover.gcs().send_mission_item_reached_message(0);
}
// we have reached the destination so stay here
if (rover.is_boat()) {
if (!start_loiter()) {
@ -39,6 +41,8 @@ void ModeGuided::update()
} else {
stop_vehicle();
}
// update distance to destination
_distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
}
break;
}
@ -110,10 +114,19 @@ void ModeGuided::update()
// return distance (in meters) to destination
float ModeGuided::get_distance_to_destination() const
{
if (_guided_mode != Guided_WP || _reached_destination) {
switch (_guided_mode) {
case Guided_WP:
return _distance_to_destination;
case Guided_HeadingAndSpeed:
case Guided_TurnRateAndSpeed:
return 0.0f;
case Guided_Loiter:
rover.mode_loiter.get_distance_to_destination();
break;
}
return _distance_to_destination;
// we should never reach here but just in case, return 0
return 0.0f;
}
// return true if vehicle has reached or even passed destination
@ -122,12 +135,10 @@ bool ModeGuided::reached_destination() const
switch (_guided_mode) {
case Guided_WP:
return _reached_destination;
break;
case Guided_HeadingAndSpeed:
case Guided_TurnRateAndSpeed:
case Guided_Loiter:
return true;
break;
}
// we should never reach here but just in case, return true is the safer option
@ -138,12 +149,12 @@ bool ModeGuided::reached_destination() const
void ModeGuided::set_desired_location(const struct Location& destination,
float next_leg_bearing_cd)
{
// call parent
Mode::set_desired_location(destination, next_leg_bearing_cd);
if (g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd)) {
// handle guided specific initialisation and logging
_guided_mode = ModeGuided::Guided_WP;
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_destination.lat, _destination.lng, 0), Vector3f(_desired_speed, 0.0f, 0.0f));
// handle guided specific initialisation and logging
_guided_mode = ModeGuided::Guided_WP;
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_desired_speed(), 0.0f, 0.0f));
}
}
// set desired attitude

42
APMrover2/mode_rtl.cpp
View File

@ -9,37 +9,47 @@ bool ModeRTL::_enter()
}
// initialise waypoint speed
set_desired_speed_to_default(true);
if (is_positive(g2.rtl_speed)) {
g2.wp_nav.set_desired_speed(g2.rtl_speed);
} else {
g2.wp_nav.set_desired_speed_to_default();
}
// set target to the closest rally point or home
#if AP_RALLY == ENABLED
set_desired_location(rover.g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt));
g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt));
#else
// set destination
set_desired_location(rover.home);
g2.wp_nav.set_desired_location(rover.home);
#endif
sent_notification = false;
return true;
}
void ModeRTL::update()
{
// calculate distance to home
_distance_to_destination = rover.current_loc.get_distance(_destination);
const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius;
// check if we've reached the destination
if (!_reached_destination && (near_wp || rover.current_loc.past_interval_finish_line(_origin, _destination))) {
// trigger reached
_reached_destination = true;
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
}
// determine if we should keep navigating
if (!_reached_destination || (rover.is_boat() && !near_wp)) {
// continue driving towards destination
calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_origin, _destination, _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
if (!g2.wp_nav.reached_destination() || rover.is_boat()) {
// update navigation controller
navigate_to_waypoint();
} else {
// send notification
if (!sent_notification) {
sent_notification = true;
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
}
// we've reached destination so stop
stop_vehicle();
// update distance to destination
_distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
}
}
bool ModeRTL::reached_destination() const
{
return g2.wp_nav.reached_destination();
}

29
APMrover2/mode_smart_rtl.cpp
View File

@ -14,15 +14,20 @@ bool ModeSmartRTL::_enter()
return false;
}
// initialise waypoint speed
set_desired_speed_to_default(true);
// set desired location to reasonable stopping point
if (!g2.wp_nav.set_desired_location_to_stopping_location()) {
return false;
}
// init location target
set_desired_location(rover.current_loc);
// initialise waypoint speed
if (is_positive(g2.rtl_speed)) {
g2.wp_nav.set_desired_speed(g2.rtl_speed);
} else {
g2.wp_nav.set_desired_speed_to_default();
}
// init state
smart_rtl_state = SmartRTL_WaitForPathCleanup;
_reached_destination = false;
return true;
}
@ -52,20 +57,19 @@ void ModeSmartRTL::update()
}
_load_point = false;
// set target destination to new point
if (!set_desired_location_NED(next_point)) {
if (!g2.wp_nav.set_desired_location_NED(next_point)) {
// this failure should never happen but we add it just in case
gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
smart_rtl_state = SmartRTL_Failure;
}
}
// update navigation controller
navigate_to_waypoint();
// check if we've reached the next point
_distance_to_destination = rover.current_loc.get_distance(_destination);
if (_distance_to_destination <= rover.g.waypoint_radius || rover.current_loc.past_interval_finish_line(_origin, _destination)) {
if (g2.wp_nav.reached_destination()) {
_load_point = true;
}
// continue driving towards destination
calc_steering_to_waypoint(_origin, _destination, _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
break;
case SmartRTL_StopAtHome:
@ -73,8 +77,7 @@ void ModeSmartRTL::update()
_reached_destination = true;
if (rover.is_boat()) {
// boats attempt to hold position at home
calc_steering_to_waypoint(rover.current_loc, _destination, _reversed);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
navigate_to_waypoint();
} else {
// rovers stop
stop_vehicle();

8
APMrover2/sailboat.cpp
View File

@ -67,7 +67,7 @@ float Rover::sailboat_get_VMG() const
if (!g2.attitude_control.get_forward_speed(speed)) {
return 0.0f;
}
return (speed * cosf(wrap_PI(radians(nav_controller->target_bearing_cd()) - ahrs.yaw)));
return (speed * cosf(wrap_PI(radians(g2.wp_nav.wp_bearing_cd() * 0.01f) - ahrs.yaw)));
}
// handle user initiated tack while in acro mode
@ -151,14 +151,14 @@ float Rover::sailboat_calc_heading(float desired_heading_cd)
// trigger tack if cross track error larger than waypoint_overshoot parameter
// this effectively defines a 'corridor' of width 2*waypoint_overshoot that the boat will stay within
if ((fabsf(rover.nav_controller->crosstrack_error()) >= g.waypoint_overshoot) && !is_zero(g.waypoint_overshoot) && !sailboat_tacking()) {
if ((fabsf(g2.wp_nav.crosstrack_error()) >= g2.wp_nav.get_overshoot()) && !is_zero(g2.wp_nav.get_overshoot()) && !sailboat_tacking()) {
// make sure the new tack will reduce the cross track error
// if were on starboard tack we are traveling towards the left hand boundary
if (is_positive(rover.nav_controller->crosstrack_error()) && (current_tack == TACK_STARBOARD)) {
if (is_positive(g2.wp_nav.crosstrack_error()) && (current_tack == TACK_STARBOARD)) {
should_tack = true;
}
// if were on port tack we are traveling towards the right hand boundary
if (is_negative(rover.nav_controller->crosstrack_error()) && (current_tack == TACK_PORT)) {
if (is_negative(g2.wp_nav.crosstrack_error()) && (current_tack == TACK_PORT)) {
should_tack = true;
}
}