Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: move set_desired_speed to each mode

This commit is contained in:
Randy Mackay 2019-11-04 17:31:13 +09:00
parent 45682550e7
commit 129651b7e4
7 changed files with 108 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
APMrover2/mode.cpp
View File

@ -209,17 +209,6 @@ bool Mode::set_desired_location(const struct Location& destination, float next_l
return true;
}
// set desired heading and speed
void Mode::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
{
// handle initialisation
_reached_destination = false;
// record targets
_desired_yaw_cd = yaw_angle_cd;
_desired_speed = target_speed;
}
// get default speed for this mode (held in WP_SPEED or RTL_SPEED)
float Mode::get_speed_default(bool rtl) const
{
@ -230,22 +219,6 @@ float Mode::get_speed_default(bool rtl) const
return g2.wp_nav.get_default_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);
}
// set desired speed in m/s
bool Mode::set_desired_speed(float speed)
{
if (!is_negative(speed)) {
_desired_speed = speed;
return true;
}
return false;
}
// execute the mission in reverse (i.e. backing up)
void Mode::set_reversed(bool value)
{

36
APMrover2/mode.h
View File

@ -115,19 +115,12 @@ public:
// 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; }
// set desired heading and speed - supported in Auto and Guided modes
virtual void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed);
// get default speed for this mode (held in CRUISE_SPEED, WP_SPEED or RTL_SPEED)
// rtl argument should be true if called from RTL or SmartRTL modes (handled here to avoid duplication)
float get_speed_default(bool rtl = false) const;
// set desired speed in m/s
bool set_desired_speed(float speed);
// restore desired speed to default from parameter values (CRUISE_SPEED or WP_SPEED)
// rtl argument should be true if called from RTL or SmartRTL modes (handled here to avoid duplication)
void set_desired_speed_to_default(bool rtl = false);
virtual bool set_desired_speed(float speed) { return false; }
// execute the mission in reverse (i.e. backing up)
void set_reversed(bool value);
@ -207,7 +200,6 @@ protected:
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. used in Auto, Guided and Loiter
float _desired_speed; // desired speed in m/s
};
@ -258,9 +250,8 @@ public:
bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override WARN_IF_UNUSED;
bool reached_destination() const override;
// heading and speed control
void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override;
bool reached_heading();
// set desired speed in m/s
bool set_desired_speed(float speed) override;
// start RTL (within auto)
void start_RTL();
@ -325,6 +316,9 @@ private:
};
bool auto_triggered; // true when auto has been triggered to start
// HeadingAndSpeed sub mode variables
float _desired_speed; // desired speed in HeadingAndSpeed submode
bool _reached_heading; // true when vehicle has reached desired heading in TurnToHeading sub mode
// Loiter control
@ -376,12 +370,15 @@ public:
// return true if vehicle has reached destination
bool reached_destination() const override;
// set desired speed in m/s
bool set_desired_speed(float speed) override;
// get or set desired location
bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override WARN_IF_UNUSED;
// set desired heading and speed
void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override;
void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed);
// set desired heading-delta, turn-rate and speed
void set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed);
@ -414,6 +411,7 @@ protected:
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
float _desired_speed; // desired speed used only in HeadingAndSpeed submode
// limits
struct {
@ -472,6 +470,7 @@ protected:
bool _enter() override;
Location _destination; // target location to hold position around
float _desired_speed; // desired speed (ramped down from initial speed to zero)
};
class ModeManual : public Mode
@ -518,6 +517,9 @@ public:
float get_distance_to_destination() const override { return _distance_to_destination; }
bool reached_destination() const override;
// set desired speed in m/s
bool set_desired_speed(float speed) override;
protected:
bool _enter() override;
@ -547,6 +549,9 @@ public:
float get_distance_to_destination() const override { return _distance_to_destination; }
bool reached_destination() const override { return smart_rtl_state == SmartRTL_StopAtHome; }
// set desired speed in m/s
bool set_desired_speed(float speed) override;
// save current position for use by the smart_rtl flight mode
void save_position();
@ -631,10 +636,15 @@ public:
// return distance (in meters) to destination
float get_distance_to_destination() const override;
// set desired speed in m/s
bool set_desired_speed(float speed) override;
protected:
bool _enter() override;
void _exit() override;
float _desired_speed; // desired speed in m/s
};
class ModeSimple : public Mode

50
APMrover2/mode_auto.cpp
View File

@ -202,24 +202,28 @@ bool ModeAuto::reached_destination() const
return true;
}
// set desired heading in centidegrees (vehicle will turn to this heading)
void ModeAuto::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
// set desired speed in m/s
bool ModeAuto::set_desired_speed(float speed)
{
// call parent
Mode::set_desired_heading_and_speed(yaw_angle_cd, target_speed);
_submode = Auto_HeadingAndSpeed;
_reached_heading = false;
}
// return true if vehicle has reached desired heading
bool ModeAuto::reached_heading()
{
if (_submode == Auto_HeadingAndSpeed) {
return _reached_heading;
}
// we should never reach here but just in case, return true to allow missions to continue
switch (_submode) {
case Auto_WP:
case Auto_Stop:
if (!is_negative(speed)) {
g2.wp_nav.set_desired_speed(speed);
return true;
}
return false;
case Auto_HeadingAndSpeed:
_desired_speed = speed;
return true;
case Auto_RTL:
return rover.mode_rtl.set_desired_speed(speed);
case Auto_Loiter:
return rover.mode_loiter.set_desired_speed(speed);
case Auto_Guided:
return rover.mode_guided.set_desired_speed(speed);
}
return false;
}
// start RTL (within auto)
@ -604,9 +608,13 @@ void ModeAuto::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd)
desired_heading_cd = cmd.content.set_yaw_speed.angle_deg * 100.0f;
}
// set auto target
// set targets
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));
_desired_speed = constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max);
_desired_yaw_cd = desired_heading_cd;
_reached_heading = false;
_reached_destination = false;
_submode = Auto_HeadingAndSpeed;
}
/********************************************************************************/
@ -700,7 +708,11 @@ bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
// verify_yaw - return true if we have reached the desired heading
bool ModeAuto::verify_nav_set_yaw_speed()
{
return reached_heading();
if (_submode == Auto_HeadingAndSpeed) {
return _reached_heading;
}
// we should never reach here but just in case, return true to allow missions to continue
return true;
}
/********************************************************************************/

14
APMrover2/mode_follow.cpp
View File

@ -8,8 +8,8 @@ bool ModeFollow::_enter()
return false;
}
// initialise waypoint speed
set_desired_speed_to_default();
// initialise speed to waypoint speed
_desired_speed = g2.wp_nav.get_default_speed();
return true;
}
@ -85,3 +85,13 @@ float ModeFollow::get_distance_to_destination() const
{
return g2.follow.get_distance_to_target();
}
// set desired speed in m/s
bool ModeFollow::set_desired_speed(float speed)
{
if (is_negative(speed)) {
return false;
}
_desired_speed = speed;
return true;
}

25
APMrover2/mode_guided.cpp
View File

@ -144,6 +144,26 @@ bool ModeGuided::reached_destination() const
return true;
}
// set desired speed in m/s
bool ModeGuided::set_desired_speed(float speed)
{
switch (_guided_mode) {
case Guided_WP:
if (!is_negative(speed)) {
g2.wp_nav.set_desired_speed(speed);
return true;
}
return false;
case Guided_HeadingAndSpeed:
case Guided_TurnRateAndSpeed:
// speed is set from mavlink message
return false;
case Guided_Loiter:
return rover.mode_loiter.set_desired_speed(speed);
}
return false;
}
// get desired location
bool ModeGuided::get_desired_location(Location& destination) const
{
@ -185,10 +205,7 @@ bool ModeGuided::set_desired_location(const struct Location& destination,
// set desired attitude
void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
{
// call parent
Mode::set_desired_heading_and_speed(yaw_angle_cd, target_speed);
// handle guided specific initialisation and logging
// initialisation and logging
_guided_mode = ModeGuided::Guided_HeadingAndSpeed;
_des_att_time_ms = AP_HAL::millis();
_reached_destination = false;

10
APMrover2/mode_rtl.cpp
View File

@ -81,3 +81,13 @@ bool ModeRTL::reached_destination() const
{
return g2.wp_nav.reached_destination();
}
// set desired speed in m/s
bool ModeRTL::set_desired_speed(float speed)
{
if (is_negative(speed)) {
return false;
}
g2.wp_nav.set_desired_speed(speed);
return true;
}

10
APMrover2/mode_smart_rtl.cpp
View File

@ -115,6 +115,16 @@ bool ModeSmartRTL::get_desired_location(Location& destination) const
return false;
}
// set desired speed in m/s
bool ModeSmartRTL::set_desired_speed(float speed)
{
if (is_negative(speed)) {
return false;
}
g2.wp_nav.set_desired_speed(speed);
return true;
}
// save current position for use by the smart_rtl flight mode
void ModeSmartRTL::save_position()
{