diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 5c35ed87c2..5a155755c7 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -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 }; diff --git a/Rover/Parameters.h b/Rover/Parameters.h index 83f718b91e..25a89e0c46 100644 --- a/Rover/Parameters.h +++ b/Rover/Parameters.h @@ -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[]; diff --git a/Rover/Rover.h b/Rover/Rover.h index b06a257033..3310809815 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -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; diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 1509b72436..5f70481c19 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -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; diff --git a/Rover/mode.h b/Rover/mode.h index 761602b6ff..a56250dddc 100644 --- a/Rover/mode.h +++ b/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 { diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index 0b63954796..c8bdfe82f5 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -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 /********************************************************************************/ diff --git a/Rover/mode_circle.cpp b/Rover/mode_circle.cpp new file mode 100644 index 0000000000..8bedb2b16b --- /dev/null +++ b/Rover/mode_circle.cpp @@ -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; +}