mirror of https://github.com/ArduPilot/ardupilot
Rover: add circle mode
also auto mode support loiter turns
This commit is contained in:
Randy Mackay
Andrew Tridgell
parent
b38a334056
commit
8c173a9d9a
|
|
@ -27,7 +27,7 @@ const AP_Param::Info Rover::var_info[] = {
|
|||
// @Param: INITIAL_MODE
|
||||
// @DisplayName: Initial driving mode
|
||||
// @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. Usually used in combination with when AUTO_TRIGGER_PIN or AUTO_KICKSTART.
|
||||
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,8:Dock,10:Auto,11:RTL,12:SmartRTL,15:Guided
|
||||
// @CopyValuesFrom: MODE1
|
||||
// @User: Advanced
|
||||
GSCALAR(initial_mode, "INITIAL_MODE", Mode::Number::MANUAL),
|
||||
|
||||
|
|
@ -171,7 +171,7 @@ const AP_Param::Info Rover::var_info[] = {
|
|||
|
||||
// @Param: MODE1
|
||||
// @DisplayName: Mode1
|
||||
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,8:Dock,10:Auto,11:RTL,12:SmartRTL,15:Guided
|
||||
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,8:Dock,9:Circle,10:Auto,11:RTL,12:SmartRTL,15:Guided
|
||||
// @User: Standard
|
||||
// @Description: Driving mode for switch position 1 (910 to 1230 and above 2049)
|
||||
GSCALAR(mode1, "MODE1", Mode::Number::MANUAL),
|
||||
|
|
@ -692,6 +692,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
|
|||
// @User: Standard
|
||||
AP_GROUPINFO("FS_GCS_TIMEOUT", 56, ParametersG2, fs_gcs_timeout, 5),
|
||||
|
||||
// @Group: CIRC
|
||||
// @Path: mode_circle.cpp
|
||||
AP_SUBGROUPINFO(mode_circle, "CIRC", 57, ParametersG2, ModeCircle),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -436,6 +436,8 @@ public:
|
|||
|
||||
// FS GCS timeout trigger time
|
||||
AP_Float fs_gcs_timeout;
|
||||
|
||||
class ModeCircle mode_circle;
|
||||
};
|
||||
|
||||
extern const AP_Param::Info var_info[];
|
||||
|
|
|
|||
|
|
@ -82,6 +82,7 @@ public:
|
|||
friend class Mode;
|
||||
friend class ModeAcro;
|
||||
friend class ModeAuto;
|
||||
friend class ModeCircle;
|
||||
friend class ModeGuided;
|
||||
friend class ModeHold;
|
||||
friend class ModeLoiter;
|
||||
|
|
|
|||
|
|
@ -537,6 +537,9 @@ Mode *Rover::mode_from_mode_num(const enum Mode::Number num)
|
|||
case Mode::Number::SIMPLE:
|
||||
ret = &mode_simple;
|
||||
break;
|
||||
case Mode::Number::CIRCLE:
|
||||
ret = &g2.mode_circle;
|
||||
break;
|
||||
case Mode::Number::AUTO:
|
||||
ret = &mode_auto;
|
||||
break;
|
||||
|
|
|
|||
91
Rover/mode.h
91
Rover/mode.h
|
|
@ -22,6 +22,7 @@ public:
|
|||
#if MODE_DOCK_ENABLED == ENABLED
|
||||
DOCK = 8,
|
||||
#endif
|
||||
CIRCLE = 9,
|
||||
AUTO = 10,
|
||||
RTL = 11,
|
||||
SMART_RTL = 12,
|
||||
|
|
@ -252,6 +253,12 @@ public:
|
|||
// return if external control is allowed in this mode (Guided or Guided-within-Auto)
|
||||
bool in_guided_mode() const override { return _submode == Auto_Guided || _submode == Auto_NavScriptTime; }
|
||||
|
||||
// return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
|
||||
float wp_bearing() const override;
|
||||
float nav_bearing() const override;
|
||||
float crosstrack_error() const override;
|
||||
float get_desired_lat_accel() const override;
|
||||
|
||||
// return distance (in meters) to destination
|
||||
float get_distance_to_destination() const override;
|
||||
|
||||
|
|
@ -295,6 +302,7 @@ protected:
|
|||
Auto_Guided, // handover control to external navigation system from within auto mode
|
||||
Auto_Stop, // stop the vehicle as quickly as possible
|
||||
Auto_NavScriptTime, // accept targets from lua scripts while NAV_SCRIPT_TIME commands are executing
|
||||
Auto_Circle, // circle a given location
|
||||
} _submode;
|
||||
|
||||
private:
|
||||
|
|
@ -322,6 +330,8 @@ private:
|
|||
bool verify_loiter_time(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_nav_set_yaw_speed();
|
||||
bool do_circle(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_circle(const AP_Mission::Mission_Command& cmd);
|
||||
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
|
||||
void do_within_distance(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_wait_delay();
|
||||
|
|
@ -385,6 +395,87 @@ private:
|
|||
AP_Mission_ChangeDetector mis_change_detector;
|
||||
};
|
||||
|
||||
class ModeCircle : public Mode
|
||||
{
|
||||
public:
|
||||
|
||||
// need a constructor for parameters
|
||||
ModeCircle();
|
||||
|
||||
// Does not allow copies
|
||||
CLASS_NO_COPY(ModeCircle);
|
||||
|
||||
uint32_t mode_number() const override { return CIRCLE; }
|
||||
const char *name4() const override { return "CIRC"; }
|
||||
|
||||
// initialise with specific center location, radius (in meters) and direction
|
||||
// replaces use of _enter when initialised from within Auto mode
|
||||
bool set_center(const Location& center_loc, float radius_m, bool dir_ccw);
|
||||
|
||||
// methods that affect movement of the vehicle in this mode
|
||||
void update() override;
|
||||
|
||||
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;
|
||||
float nav_bearing() const override;
|
||||
float crosstrack_error() const override { return dist_to_edge_m; }
|
||||
float get_desired_lat_accel() const override;
|
||||
|
||||
// set desired speed in m/s
|
||||
bool set_desired_speed(float speed_ms) override;
|
||||
|
||||
// return distance (in meters) to destination
|
||||
float get_distance_to_destination() const override { return _distance_to_destination; }
|
||||
|
||||
// get or set desired location
|
||||
bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
|
||||
|
||||
// return total angle in radians that vehicle has circled
|
||||
// fabsf is used so that full rotations in either direction are counted
|
||||
float get_angle_total_rad() const { return fabsf(angle_total_rad); }
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
protected:
|
||||
|
||||
AP_Float radius; // circle radius in meters
|
||||
AP_Float speed; // vehicle speed in m/s. If zero uses WP_SPEED
|
||||
AP_Int8 direction; // direction 0:clockwise, 1:counter-clockwise
|
||||
|
||||
// initialise mode
|
||||
bool _enter() override;
|
||||
|
||||
// initialise target_yaw_rad using the vehicle's position and yaw
|
||||
// if there is no current position estimate target_yaw_rad is set to vehicle yaw
|
||||
void init_target_yaw_rad();
|
||||
|
||||
// enum for Direction parameter
|
||||
enum class Direction {
|
||||
CW = 0,
|
||||
CCW = 1
|
||||
};
|
||||
|
||||
// local members
|
||||
struct {
|
||||
Location center_loc; // circle center as a Location
|
||||
Vector2f center_pos; // circle center as an offset (in meters) from the EKF origin
|
||||
float radius; // circle radius
|
||||
float speed; // desired speed around circle in m/s
|
||||
Direction dir; // direction, 0:clockwise, 1:counter-clockwise
|
||||
} config;
|
||||
struct {
|
||||
float speed; // vehicle's target speed around circle in m/s
|
||||
float yaw_rad; // earth-frame angle of tarrget point on the circle
|
||||
Vector2p pos; // latest position target sent to position controller
|
||||
Vector2f vel; // latest velocity target sent to position controller
|
||||
Vector2f accel; // latest accel target sent to position controller
|
||||
} target;
|
||||
float angle_total_rad; // total angle in radians that vehicle has circled
|
||||
bool reached_edge; // true once vehicle has reached edge of circle
|
||||
float dist_to_edge_m; // distance to edge of circle in meters (equivalent to crosstrack error)
|
||||
};
|
||||
|
||||
class ModeGuided : public Mode
|
||||
{
|
||||
|
|
|
|||
|
|
@ -139,6 +139,10 @@ void ModeAuto::update()
|
|||
case Auto_NavScriptTime:
|
||||
rover.mode_guided.update();
|
||||
break;
|
||||
|
||||
case Auto_Circle:
|
||||
rover.g2.mode_circle.update();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -152,6 +156,102 @@ void ModeAuto::calc_throttle(float target_speed, bool avoidance_enabled)
|
|||
Mode::calc_throttle(target_speed, avoidance_enabled);
|
||||
}
|
||||
|
||||
// return heading (in degrees) to target destination (aka waypoint)
|
||||
float ModeAuto::wp_bearing() const
|
||||
{
|
||||
switch (_submode) {
|
||||
case Auto_WP:
|
||||
return g2.wp_nav.wp_bearing_cd() * 0.01f;
|
||||
case Auto_HeadingAndSpeed:
|
||||
case Auto_Stop:
|
||||
return 0.0f;
|
||||
case Auto_RTL:
|
||||
return rover.mode_rtl.wp_bearing();
|
||||
case Auto_Loiter:
|
||||
return rover.mode_loiter.wp_bearing();
|
||||
case Auto_Guided:
|
||||
case Auto_NavScriptTime:
|
||||
return rover.mode_guided.wp_bearing();
|
||||
case Auto_Circle:
|
||||
return rover.g2.mode_circle.wp_bearing();
|
||||
}
|
||||
|
||||
// this line should never be reached
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// return short-term target heading in degrees (i.e. target heading back to line between waypoints)
|
||||
float ModeAuto::nav_bearing() const
|
||||
{
|
||||
switch (_submode) {
|
||||
case Auto_WP:
|
||||
return g2.wp_nav.nav_bearing_cd() * 0.01f;
|
||||
case Auto_HeadingAndSpeed:
|
||||
case Auto_Stop:
|
||||
return 0.0f;
|
||||
case Auto_RTL:
|
||||
return rover.mode_rtl.nav_bearing();
|
||||
case Auto_Loiter:
|
||||
return rover.mode_loiter.nav_bearing();
|
||||
case Auto_Guided:
|
||||
case Auto_NavScriptTime:
|
||||
return rover.mode_guided.nav_bearing();
|
||||
case Auto_Circle:
|
||||
return rover.g2.mode_circle.nav_bearing();
|
||||
}
|
||||
|
||||
// this line should never be reached
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// return cross track error (i.e. vehicle's distance from the line between waypoints)
|
||||
float ModeAuto::crosstrack_error() const
|
||||
{
|
||||
switch (_submode) {
|
||||
case Auto_WP:
|
||||
return g2.wp_nav.crosstrack_error();
|
||||
case Auto_HeadingAndSpeed:
|
||||
case Auto_Stop:
|
||||
return 0.0f;
|
||||
case Auto_RTL:
|
||||
return rover.mode_rtl.crosstrack_error();
|
||||
case Auto_Loiter:
|
||||
return rover.mode_loiter.crosstrack_error();
|
||||
case Auto_Guided:
|
||||
case Auto_NavScriptTime:
|
||||
return rover.mode_guided.crosstrack_error();
|
||||
case Auto_Circle:
|
||||
return rover.g2.mode_circle.crosstrack_error();
|
||||
}
|
||||
|
||||
// this line should never be reached
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// return desired lateral acceleration
|
||||
float ModeAuto::get_desired_lat_accel() const
|
||||
{
|
||||
switch (_submode) {
|
||||
case Auto_WP:
|
||||
return g2.wp_nav.get_lat_accel();
|
||||
case Auto_HeadingAndSpeed:
|
||||
case Auto_Stop:
|
||||
return 0.0f;
|
||||
case Auto_RTL:
|
||||
return rover.mode_rtl.get_desired_lat_accel();
|
||||
case Auto_Loiter:
|
||||
return rover.mode_loiter.get_desired_lat_accel();
|
||||
case Auto_Guided:
|
||||
case Auto_NavScriptTime:
|
||||
return rover.mode_guided.get_desired_lat_accel();
|
||||
case Auto_Circle:
|
||||
return rover.g2.mode_circle.get_desired_lat_accel();
|
||||
}
|
||||
|
||||
// this line should never be reached
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// return distance (in meters) to destination
|
||||
float ModeAuto::get_distance_to_destination() const
|
||||
{
|
||||
|
|
@ -169,6 +269,8 @@ float ModeAuto::get_distance_to_destination() const
|
|||
case Auto_Guided:
|
||||
case Auto_NavScriptTime:
|
||||
return rover.mode_guided.get_distance_to_destination();
|
||||
case Auto_Circle:
|
||||
return rover.g2.mode_circle.get_distance_to_destination();
|
||||
}
|
||||
|
||||
// this line should never be reached
|
||||
|
|
@ -196,6 +298,8 @@ bool ModeAuto::get_desired_location(Location& destination) const
|
|||
case Auto_Guided:
|
||||
case Auto_NavScriptTime:
|
||||
return rover.mode_guided.get_desired_location(destination);
|
||||
case Auto_Circle:
|
||||
return rover.g2.mode_circle.get_desired_location(destination);
|
||||
}
|
||||
|
||||
// we should never reach here but just in case
|
||||
|
|
@ -236,7 +340,8 @@ bool ModeAuto::reached_destination() const
|
|||
case Auto_Guided:
|
||||
case Auto_NavScriptTime:
|
||||
return rover.mode_guided.reached_destination();
|
||||
break;
|
||||
case Auto_Circle:
|
||||
return rover.g2.mode_circle.reached_destination();
|
||||
}
|
||||
|
||||
// we should never reach here but just in case, return true to allow missions to continue
|
||||
|
|
@ -260,6 +365,8 @@ bool ModeAuto::set_desired_speed(float speed)
|
|||
case Auto_Guided:
|
||||
case Auto_NavScriptTime:
|
||||
return rover.mode_guided.set_desired_speed(speed);
|
||||
case Auto_Circle:
|
||||
return rover.g2.mode_circle.set_desired_speed(speed);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
|
@ -422,6 +529,9 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
|
|||
case MAV_CMD_NAV_LOITER_TIME: // Loiter for specified time
|
||||
return do_nav_wp(cmd, true);
|
||||
|
||||
case MAV_CMD_NAV_LOITER_TURNS:
|
||||
return do_circle(cmd);
|
||||
|
||||
case MAV_CMD_NAV_GUIDED_ENABLE: // accept navigation commands from external nav computer
|
||||
do_nav_guided_enable(cmd);
|
||||
break;
|
||||
|
|
@ -564,6 +674,9 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
|
|||
case MAV_CMD_NAV_LOITER_UNLIM:
|
||||
return verify_loiter_unlimited(cmd);
|
||||
|
||||
case MAV_CMD_NAV_LOITER_TURNS:
|
||||
return verify_circle(cmd);
|
||||
|
||||
case MAV_CMD_NAV_LOITER_TIME:
|
||||
return verify_loiter_time(cmd);
|
||||
|
||||
|
|
@ -802,6 +915,34 @@ bool ModeAuto::verify_nav_set_yaw_speed()
|
|||
return true;
|
||||
}
|
||||
|
||||
bool ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// retrieve and sanitize target location
|
||||
Location circle_center = cmd.content.location;
|
||||
circle_center.sanitize(rover.current_loc);
|
||||
|
||||
// calculate radius
|
||||
uint16_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
|
||||
if (cmd.id == MAV_CMD_NAV_LOITER_TURNS &&
|
||||
cmd.type_specific_bits & (1U << 0)) {
|
||||
// special storage handling allows for larger radii
|
||||
circle_radius_m *= 10;
|
||||
}
|
||||
|
||||
// initialise circle mode
|
||||
if (g2.mode_circle.set_center(circle_center, circle_radius_m, cmd.content.location.loiter_ccw)) {
|
||||
_submode = Auto_Circle;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// check if we have completed circling
|
||||
return ((g2.mode_circle.get_angle_total_rad() / M_2PI) >= LOWBYTE(cmd.p1));
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
// Condition (May) commands
|
||||
/********************************************************************************/
|
||||
|
|
|
|||
|
|
@ -0,0 +1,237 @@
|
|||
#include "Rover.h"
|
||||
|
||||
#define AR_CIRCLE_ACCEL_DEFAULT 1.0 // default acceleration in m/s/s if not specified by user
|
||||
#define AR_CIRCLE_RADIUS_MIN 0.5 // minimum radius in meters
|
||||
#define AR_CIRCLE_REACHED_EDGE_DIST 1.0 // vehicle has reached edge if within 1m
|
||||
|
||||
const AP_Param::GroupInfo ModeCircle::var_info[] = {
|
||||
|
||||
// @Param: _RADIUS
|
||||
// @DisplayName: Circle Radius
|
||||
// @Description: Vehicle will circle the center at this distance
|
||||
// @Units: m
|
||||
// @Range: 0 100
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("_RADIUS", 1, ModeCircle, radius, 20),
|
||||
|
||||
// @Param: _SPEED
|
||||
// @DisplayName: Circle Speed
|
||||
// @Description: Vehicle will move at this speed around the circle. If set to zero WP_SPEED will be used
|
||||
// @Units: m/s
|
||||
// @Range: 0 10
|
||||
// @Increment: 0.1
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("_SPEED", 2, ModeCircle, speed, 0),
|
||||
|
||||
// @Param: _DIR
|
||||
// @DisplayName: Circle Direction
|
||||
// @Description: Circle Direction
|
||||
// @Values: 0:Clockwise, 1:Counter-Clockwise
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("_DIR", 3, ModeCircle, direction, 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
ModeCircle::ModeCircle() : Mode()
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
// initialise with specific center location, radius (in meters) and direction
|
||||
// replaces use of _enter when initialised from within Auto mode
|
||||
bool ModeCircle::set_center(const Location& center_loc, float radius_m, bool dir_ccw)
|
||||
{
|
||||
Vector2f center_pos_cm;
|
||||
if (!center_loc.get_vector_xy_from_origin_NE(center_pos_cm)) {
|
||||
return false;
|
||||
}
|
||||
if (!_enter()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// convert center position from cm to m
|
||||
config.center_pos = center_pos_cm * 0.01;
|
||||
|
||||
// set radius
|
||||
config.radius = MAX(fabsf(radius_m), AR_CIRCLE_RADIUS_MIN);
|
||||
|
||||
// set direction
|
||||
config.dir = dir_ccw ? Direction::CCW : Direction::CW;
|
||||
|
||||
// set target yaw rad (target point on circle)
|
||||
init_target_yaw_rad();
|
||||
|
||||
// record center as location (only used for reporting)
|
||||
config.center_loc = center_loc;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// initialize dock mode
|
||||
bool ModeCircle::_enter()
|
||||
{
|
||||
// capture starting point and yaw
|
||||
if (!AP::ahrs().get_relative_position_NE_origin(config.center_pos)) {
|
||||
return false;
|
||||
}
|
||||
config.radius = MAX(fabsf(radius), AR_CIRCLE_RADIUS_MIN);
|
||||
config.dir = (direction == 1) ? Direction::CCW : Direction::CW;
|
||||
config.speed = is_positive(speed) ? speed : g2.wp_nav.get_default_speed();
|
||||
target.yaw_rad = AP::ahrs().get_yaw();
|
||||
target.speed = 0;
|
||||
|
||||
// calculate speed, accel and jerk limits
|
||||
// otherwise the vehicle uses wp_nav default speed limit
|
||||
float atc_accel_max = MIN(g2.attitude_control.get_accel_max(), g2.attitude_control.get_decel_max());
|
||||
if (!is_positive(atc_accel_max)) {
|
||||
atc_accel_max = AR_CIRCLE_ACCEL_DEFAULT;
|
||||
}
|
||||
const float accel_max = is_positive(g2.wp_nav.get_default_accel()) ? MIN(g2.wp_nav.get_default_accel(), atc_accel_max) : atc_accel_max;
|
||||
const float jerk_max = is_positive(g2.wp_nav.get_default_jerk()) ? g2.wp_nav.get_default_jerk() : accel_max;
|
||||
|
||||
// initialise position controller
|
||||
g2.pos_control.set_limits(config.speed, accel_max, g2.attitude_control.get_turn_lat_accel_max(), jerk_max);
|
||||
g2.pos_control.init();
|
||||
|
||||
// initialise angles covered and reached_edge state
|
||||
angle_total_rad = 0;
|
||||
reached_edge = false;
|
||||
dist_to_edge_m = 0;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// initialise target_yaw_rad using the vehicle's position and yaw
|
||||
// if there is no current position estimate target_yaw_rad is set to 0
|
||||
void ModeCircle::init_target_yaw_rad()
|
||||
{
|
||||
// if no position estimate use vehicle yaw
|
||||
Vector2f curr_pos_NE;
|
||||
if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE)) {
|
||||
target.yaw_rad = AP::ahrs().yaw;
|
||||
return;
|
||||
}
|
||||
|
||||
// calc vector from circle center to vehicle
|
||||
Vector2f center_to_veh = (curr_pos_NE - config.center_pos);
|
||||
float dist_m = center_to_veh.length();
|
||||
|
||||
// if current position is exactly at the center of the circle return vehicle yaw
|
||||
if (is_zero(dist_m)) {
|
||||
target.yaw_rad = AP::ahrs().yaw;
|
||||
} else {
|
||||
target.yaw_rad = center_to_veh.angle();
|
||||
}
|
||||
}
|
||||
|
||||
void ModeCircle::update()
|
||||
{
|
||||
// get current position
|
||||
Vector2f curr_pos;
|
||||
const bool position_ok = AP::ahrs().get_relative_position_NE_origin(curr_pos);
|
||||
|
||||
// if no position estimate stop vehicle
|
||||
if (!position_ok) {
|
||||
stop_vehicle();
|
||||
return;
|
||||
}
|
||||
|
||||
// check if vehicle has reached edge of circle
|
||||
const Vector2f center_to_veh = curr_pos - config.center_pos;
|
||||
_distance_to_destination = center_to_veh.length();
|
||||
dist_to_edge_m = fabsf(_distance_to_destination - config.radius);
|
||||
if (!reached_edge) {
|
||||
const float dist_thresh_m = MAX(rover.g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST);
|
||||
reached_edge = dist_to_edge_m <= dist_thresh_m;
|
||||
}
|
||||
|
||||
// accelerate speed up to desired speed
|
||||
const float speed_max = reached_edge ? config.speed : 0.0;
|
||||
const float speed_change_max = (g2.pos_control.get_accel_max() * rover.G_Dt);
|
||||
target.speed = constrain_float(speed_max, target.speed - speed_change_max, target.speed + speed_change_max);
|
||||
|
||||
// calculate angular rate and update target angle
|
||||
const float circumference = 2.0 * M_PI * config.radius;
|
||||
const float angular_rate_rad = (target.speed / circumference) * M_2PI * (config.dir == Direction::CW ? 1.0 : -1.0);
|
||||
const float angle_dt = angular_rate_rad * rover.G_Dt;
|
||||
target.yaw_rad = wrap_PI(target.yaw_rad + angle_dt);
|
||||
angle_total_rad += angle_dt;
|
||||
|
||||
// calculate target point's position, velocity and acceleration
|
||||
target.pos = config.center_pos.topostype();
|
||||
target.pos.offset_bearing(degrees(target.yaw_rad), config.radius);
|
||||
|
||||
// velocity is perpendicular to angle from the circle's center to the target point on the edge of the circle
|
||||
target.vel = Vector2f(target.speed, 0);
|
||||
target.vel.rotate(target.yaw_rad + radians(90));
|
||||
|
||||
// acceleration is towards center of circle and is speed^2 / radius
|
||||
target.accel = Vector2f(sq(target.speed) / config.radius, 0);
|
||||
target.accel.rotate(target.yaw_rad + radians(180));
|
||||
|
||||
g2.pos_control.input_pos_vel_accel_target(target.pos, target.vel, target.accel, rover.G_Dt);
|
||||
g2.pos_control.update(rover.G_Dt);
|
||||
|
||||
// get desired speed and turn rate from pos_control
|
||||
const float desired_speed = g2.pos_control.get_desired_speed();
|
||||
const float desired_turn_rate = g2.pos_control.get_desired_turn_rate_rads();
|
||||
|
||||
// run steering and throttle controllers
|
||||
calc_steering_from_turn_rate(desired_turn_rate);
|
||||
calc_throttle(desired_speed, true);
|
||||
}
|
||||
|
||||
// return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
|
||||
float ModeCircle::wp_bearing() const
|
||||
{
|
||||
Vector2f curr_pos_NE;
|
||||
if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE)) {
|
||||
return 0;
|
||||
}
|
||||
// calc vector from circle center to vehicle
|
||||
Vector2f veh_to_center = (config.center_pos - curr_pos_NE);
|
||||
if (veh_to_center.is_zero()) {
|
||||
return 0;
|
||||
}
|
||||
return degrees(veh_to_center.angle());
|
||||
}
|
||||
|
||||
float ModeCircle::nav_bearing() const
|
||||
{
|
||||
// get position error as a vector from the current position to the target position
|
||||
const Vector2p pos_error = g2.pos_control.get_pos_error();
|
||||
if (pos_error.is_zero()) {
|
||||
return 0;
|
||||
}
|
||||
return degrees(pos_error.angle());
|
||||
}
|
||||
|
||||
float ModeCircle::get_desired_lat_accel() const
|
||||
{
|
||||
return g2.pos_control.get_desired_lat_accel();
|
||||
}
|
||||
|
||||
// set desired speed in m/s
|
||||
bool ModeCircle::set_desired_speed(float speed_ms)
|
||||
{
|
||||
if (is_positive(speed_ms)) {
|
||||
config.speed = speed_ms;
|
||||
|
||||
// update position controller limits if required
|
||||
if (config.speed > g2.pos_control.get_speed_max()) {
|
||||
g2.pos_control.set_limits(config.speed, g2.pos_control.get_accel_max(), g2.pos_control.get_lat_accel_max(), g2.pos_control.get_jerk_max());
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// return desired location
|
||||
bool ModeCircle::get_desired_location(Location& destination) const
|
||||
{
|
||||
destination = config.center_loc;
|
||||
return true;
|
||||
}
|
||||
Loading…
Reference in New Issue