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

Rover: add SmartRTL mode

Browse Source 
called at 3hz from scheduler
This commit is contained in:
Peter Barker 2017-11-30 10:58:11 +09:00 committed by Randy Mackay
parent 86ce3f2b32
commit e38cefea8a
9 changed files with 149 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
APMrover2/APMrover2.cpp
View File

@ -72,6 +72,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
SCHED_TASK(update_trigger, 50, 600),
SCHED_TASK(gcs_failsafe_check, 10, 600),
SCHED_TASK(compass_accumulate, 50, 900),
SCHED_TASK(smart_rtl_update, 3, 100),
SCHED_TASK(update_notify, 50, 300),
SCHED_TASK(one_second_loop, 1, 3000),
SCHED_TASK(compass_cal_update, 50, 100),

19
APMrover2/Parameters.cpp
View File

@ -241,7 +241,7 @@ const AP_Param::Info Rover::var_info[] = {
// @Param: MODE1
// @DisplayName: Mode1
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,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", MANUAL),
@ -249,35 +249,35 @@ const AP_Param::Info Rover::var_info[] = {
// @Param: MODE2
// @DisplayName: Mode2
// @Description: Driving mode for switch position 2 (1231 to 1360)
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @User: Standard
GSCALAR(mode2, "MODE2", MANUAL),
// @Param: MODE3
// @DisplayName: Mode3
// @Description: Driving mode for switch position 3 (1361 to 1490)
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @User: Standard
GSCALAR(mode3, "MODE3", MANUAL),
// @Param: MODE4
// @DisplayName: Mode4
// @Description: Driving mode for switch position 4 (1491 to 1620)
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @User: Standard
GSCALAR(mode4, "MODE4", MANUAL),
// @Param: MODE5
// @DisplayName: Mode5
// @Description: Driving mode for switch position 5 (1621 to 1749)
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @User: Standard
GSCALAR(mode5, "MODE5", MANUAL),
// @Param: MODE6
// @DisplayName: Mode6
// @Description: Driving mode for switch position 6 (1750 to 2049)
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided
// @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided
// @User: Standard
GSCALAR(mode6, "MODE6", MANUAL),
@ -513,6 +513,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Standard
AP_GROUPINFO("ACRO_TURN_RATE", 12, ParametersG2, acro_turn_rate, 180.0f),
// @Group: SRTL_
// @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp
AP_SUBGROUPINFO(smart_rtl, "SRTL_", 13, ParametersG2, AP_SmartRTL),
AP_GROUPEND
};
@ -524,7 +528,8 @@ ParametersG2::ParametersG2(void)
#endif
beacon(rover.serial_manager),
motors(rover.ServoRelayEvents),
attitude_control(rover.ahrs)
attitude_control(rover.ahrs),
smart_rtl(rover.ahrs)
{
AP_Param::setup_object_defaults(this, var_info);
}

3
APMrover2/Parameters.h
View File

@ -319,6 +319,9 @@ public:
// acro mode turn rate maximum
AP_Float acro_turn_rate;
// Safe RTL library
AP_SmartRTL smart_rtl;
};
extern const AP_Param::Info var_info[];

4
APMrover2/Rover.h
View File

@ -67,6 +67,7 @@
#include <AP_VisualOdom/AP_VisualOdom.h>
#include <AP_WheelEncoder/AP_WheelEncoder.h>
#include <APM_Control/AR_AttitudeControl.h>
#include <AP_SmartRTL/AP_SmartRTL.h>
#include <DataFlash/DataFlash.h>
#include <Filter/AverageFilter.h> // Mode Filter from Filter library
#include <Filter/Butter.h> // Filter library - butterworth filter
@ -111,6 +112,7 @@ public:
friend class ModeSteering;
friend class ModeManual;
friend class ModeRTL;
friend class ModeSmartRTL;
Rover(void);
@ -384,6 +386,7 @@ private:
ModeAuto mode_auto;
ModeSteering mode_steering;
ModeRTL mode_rtl;
ModeSmartRTL mode_smartrtl;
// cruise throttle and speed learning
struct {
@ -568,6 +571,7 @@ private:
void change_arm_state(void);
bool arm_motors(AP_Arming::ArmingMethod method);
bool disarm_motors(void);
void smart_rtl_update();
// test.cpp
void print_hit_enter();

3
APMrover2/control_modes.cpp
View File

@ -24,6 +24,9 @@ Mode *Rover::control_mode_from_num(const enum mode num)
case RTL:
ret = &mode_rtl;
break;
case SMART_RTL:
ret = &mode_smartrtl;
break;
case GUIDED:
ret = &mode_guided;
break;

1
APMrover2/defines.h
View File

@ -33,6 +33,7 @@ enum mode {
HOLD = 4,
AUTO = 10,
RTL = 11,
SMART_RTL = 12,
GUIDED = 15,
INITIALISING = 16
};

34
APMrover2/mode.h
View File

@ -338,6 +338,40 @@ protected:
bool _enter() override;
};
class ModeSmartRTL : public Mode
{
public:
uint32_t mode_number() const override { return SMART_RTL; }
const char *name4() const override { return "SRTL"; }
// methods that affect movement of the vehicle in this mode
void update() override;
// attributes of the mode
bool is_autopilot_mode() const override { return true; }
bool failsafe_throttle_suppress() const override { return false; }
float get_distance_to_destination() const override { return _distance_to_destination; }
bool reached_destination() override { return smart_rtl_state == SmartRTL_StopAtHome; }
// save current position for use by the smart_rtl flight mode
void save_position(bool save_pos);
protected:
// Safe RTL states
enum SmartRTLState {
SmartRTL_WaitForPathCleanup,
SmartRTL_PathFollow,
SmartRTL_StopAtHome,
SmartRTL_Failure
} smart_rtl_state;
bool _enter() override;
bool _load_point;
};
class ModeSteering : public Mode
{

79
APMrover2/mode_smart_rtl.cpp Normal file
View File

@ -0,0 +1,79 @@
#include "mode.h"
#include "Rover.h"
bool ModeSmartRTL::_enter()
{
// SmartRTL requires EKF (not DCM)
Location ekf_origin;
if (!ahrs.get_origin(ekf_origin)) {
return false;
}
// refuse to enter SmartRTL if smart RTL's home has not been set
if (!g2.smart_rtl.is_active()) {
return false;
}
// RTL never reverses
rover.set_reverse(false);
// init state
smart_rtl_state = SmartRTL_WaitForPathCleanup;
_reached_destination = false;
return true;
}
void ModeSmartRTL::update()
{
switch (smart_rtl_state) {
case SmartRTL_WaitForPathCleanup:
// check if return path is computed and if yes, begin journey home
if (g2.smart_rtl.request_thorough_cleanup()) {
smart_rtl_state = SmartRTL_PathFollow;
_load_point = true;
}
// Note: this may lead to an unnecessary 20ms slow down of the vehicle (but it is unlikely)
stop_vehicle();
break;
case SmartRTL_PathFollow:
// load point if required
if (_load_point) {
Vector3f next_point;
if (!g2.smart_rtl.pop_point(next_point)) {
// if not more points, we have reached home
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
smart_rtl_state = SmartRTL_StopAtHome;
break;
}
_load_point = false;
// set target destination to new point
if (!set_desired_location_NED(next_point)) {
gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
smart_rtl_state = SmartRTL_Failure;
}
}
// check if we've reached the next point
_distance_to_destination = get_distance(rover.current_loc, _destination);
if (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination)) {
_load_point = true;
}
// continue driving towards destination
calc_steering_to_waypoint(_origin, _destination);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
break;
case SmartRTL_StopAtHome:
case SmartRTL_Failure:
_reached_destination = true;
stop_vehicle();
break;
}
}
// save current position for use by the smart_rtl flight mode
void ModeSmartRTL::save_position(bool save_pos)
{
g2.smart_rtl.update(true, save_pos);
}

12
APMrover2/system.cpp
View File

@ -133,6 +133,8 @@ void Rover::init_ardupilot()
// give AHRS the range beacon sensor
ahrs.set_beacon(&g2.beacon);
// initialize SmartRTL
g2.smart_rtl.init();
init_capabilities();
@ -332,6 +334,9 @@ bool Rover::arm_motors(AP_Arming::ArmingMethod method)
return false;
}
// Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point
g2.smart_rtl.reset_path(true);
change_arm_state();
return true;
}
@ -354,3 +359,10 @@ bool Rover::disarm_motors(void)
return true;
}
// save current position for use by the smart_rtl mode
void Rover::smart_rtl_update()
{
const bool save_position = hal.util->get_soft_armed() && (control_mode != &mode_smartrtl);
mode_smartrtl.save_position(save_position);
}