Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Trad Heli: [GSoC] Autonomous Autorotation - SITL only #12108

Merged
merged 4 commits into from
Nov 29, 2019
Merged

Trad Heli: [GSoC] Autonomous Autorotation - SITL only #12108

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
f64f641
AC_Autorotation: Created autorotation library
MattKear Nov 28, 2019
5aedb4d
Copter: Added autorotation flight mode and support
MattKear Nov 28, 2019
1557108
AP_Vehicle: Added mode change reasons for autorotation
MattKear Nov 28, 2019
fec518d
AP_Motors: Added support for autorotation
MattKear Nov 28, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 10 additions & 0 deletions ArduCopter/Copter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -250,6 +250,9 @@ void Copter::fast_loop()

#if FRAME_CONFIG == HELI_FRAME
update_heli_control_dynamics();
#if MODE_AUTOROTATE_ENABLED == ENABLED
heli_update_autorotation();
#endif
#endif //HELI_FRAME

// Inertial Nav
Expand Down Expand Up @@ -403,6 +406,13 @@ void Copter::twentyfive_hz_logging()
// log output
Log_Write_Precland();
#endif

#if MODE_AUTOROTATE_ENABLED == ENABLED
if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) {
//update autorotation log
g2.arot.Log_Write_Autorotation();
}
#endif
}

// three_hz_loop - 3.3hz loop
Expand Down
16 changes: 15 additions & 1 deletion ArduCopter/Copter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,10 @@
#define MOTOR_CLASS AP_MotorsMulticopter
#endif

#if MODE_AUTOROTATE_ENABLED == ENABLED
#include <AC_Autorotation/AC_Autorotation.h> // Autorotation controllers
#endif

#include "RC_Channel.h" // RC Channel Library

#include "GCS_Mavlink.h"
Expand All @@ -95,6 +99,7 @@
#if BEACON_ENABLED == ENABLED
#include <AP_Beacon/AP_Beacon.h>
#endif

#if AC_AVOID_ENABLED == ENABLED
#include <AC_Avoidance/AC_Avoid.h>
#endif
Expand Down Expand Up @@ -230,6 +235,7 @@ class Copter : public AP_Vehicle {
friend class ModeSystemId;
friend class ModeThrow;
friend class ModeZigZag;
friend class ModeAutorotate;

Copter(void);

Expand Down Expand Up @@ -478,6 +484,7 @@ class Copter : public AP_Vehicle {
AC_PosControl *pos_control;
AC_WPNav *wp_nav;
AC_Loiter *loiter_nav;

#if MODE_CIRCLE_ENABLED == ENABLED
AC_Circle *circle_nav;
#endif
Expand Down Expand Up @@ -575,6 +582,7 @@ class Copter : public AP_Vehicle {
typedef struct {
uint8_t dynamic_flight : 1; // 0 // true if we are moving at a significant speed (used to turn on/off leaky I terms)
uint8_t inverted_flight : 1; // 1 // true for inverted flight mode
uint8_t in_autorotation : 1; // 2 // true when heli is in autorotation
} heli_flags_t;
heli_flags_t heli_flags;

Expand Down Expand Up @@ -752,7 +760,10 @@ class Copter : public AP_Vehicle {
void update_heli_control_dynamics(void);
void heli_update_landing_swash();
void heli_update_rotor_speed_targets();

void heli_update_autorotation();
#if MODE_AUTOROTATE_ENABLED == ENABLED
void heli_set_autorotation(bool autotrotation);
#endif
// inertia.cpp
void read_inertia();

Expand Down Expand Up @@ -980,6 +991,9 @@ class Copter : public AP_Vehicle {
#if MODE_ZIGZAG_ENABLED == ENABLED
ModeZigZag mode_zigzag;
#endif
#if MODE_AUTOROTATE_ENABLED == ENABLED
ModeAutorotate mode_autorotate;
#endif

// mode.cpp
Mode *mode_from_mode_num(const Mode::Number mode);
Expand Down
25 changes: 18 additions & 7 deletions ArduCopter/Parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -262,42 +262,42 @@ const AP_Param::Info Copter::var_info[] = {
// @Param: FLTMODE1
// @DisplayName: Flight Mode 1
// @Description: Flight mode when Channel 5 pwm is <= 1230
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I wonder if we want to have the flight mode appearing in the FLTMODEx parameters. Having it here will make it appear to all users but it's not really available in our stable firmware. I think it might be better to leave it off this list and instead when people want to test it (in SITL, etc) they can type in "mode 26" to change into it. On MP it will appear as "UNKNOWN" mode which isn't great but I think for the few people who plan to use it this would be OK.

// @User: Standard
GSCALAR(flight_mode1, "FLTMODE1", (uint8_t)FLIGHT_MODE_1),

// @Param: FLTMODE2
// @DisplayName: Flight Mode 2
// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate
// @User: Standard
GSCALAR(flight_mode2, "FLTMODE2", (uint8_t)FLIGHT_MODE_2),

// @Param: FLTMODE3
// @DisplayName: Flight Mode 3
// @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate
// @User: Standard
GSCALAR(flight_mode3, "FLTMODE3", (uint8_t)FLIGHT_MODE_3),

// @Param: FLTMODE4
// @DisplayName: Flight Mode 4
// @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate
// @User: Standard
GSCALAR(flight_mode4, "FLTMODE4", (uint8_t)FLIGHT_MODE_4),

// @Param: FLTMODE5
// @DisplayName: Flight Mode 5
// @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate
// @User: Standard
GSCALAR(flight_mode5, "FLTMODE5", (uint8_t)FLIGHT_MODE_5),

// @Param: FLTMODE6
// @DisplayName: Flight Mode 6
// @Description: Flight mode when Channel 5 pwm is >=1750
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Autorotate
// @User: Standard
GSCALAR(flight_mode6, "FLTMODE6", (uint8_t)FLIGHT_MODE_6),

Expand Down Expand Up @@ -712,7 +712,7 @@ const AP_Param::Info Copter::var_info[] = {
// @Path: ../libraries/AP_OSD/AP_OSD.cpp
GOBJECT(osd, "OSD", AP_OSD),
#endif

// @Group:
// @Path: Parameters.cpp
GOBJECT(g2, "", ParametersG2),
Expand Down Expand Up @@ -955,6 +955,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Advanced
AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, 0),

#if MODE_AUTOROTATE_ENABLED == ENABLED
// @Group: AROT_
// @Path: ../libraries/AC_Autorotation/AC_Autorotation.cpp
AP_SUBGROUPINFO(arot, "AROT_", 37, ParametersG2, AC_Autorotation),
#endif



AP_GROUPEND
};

Expand Down Expand Up @@ -1041,6 +1049,9 @@ ParametersG2::ParametersG2(void)
#if MODE_SYSTEMID_ENABLED == ENABLED
,mode_systemid_ptr(&copter.mode_systemid)
#endif
#if MODE_AUTOROTATE_ENABLED == ENABLED
,arot(copter.inertial_nav)
#endif
MattKear marked this conversation as resolved.
Show resolved Hide resolved
{
AP_Param::setup_object_defaults(this, var_info);
}
Expand Down
5 changes: 5 additions & 0 deletions ArduCopter/Parameters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -615,6 +615,11 @@ class ParametersG2 {

// Failsafe options bitmask #36
AP_Int32 fs_options;

#if MODE_AUTOROTATE_ENABLED == ENABLED
// Autonmous autorotation
AC_Autorotation arot;
#endif
};

extern const AP_Param::Info var_info[];
16 changes: 16 additions & 0 deletions ArduCopter/config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -372,6 +372,22 @@
#endif

//////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////
// Autorotate - autonomous auto-rotation - helicopters only
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if FRAME_CONFIG == HELI_FRAME
#ifndef MODE_AUTOROTATE_ENABLED
# define MODE_AUTOROTATE_ENABLED !HAL_MINIMIZE_FEATURES
#endif
#else
# define MODE_AUTOROTATE_ENABLED DISABLED
#endif
#else
# define MODE_AUTOROTATE_ENABLED DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////

// Beacon support - support for local positioning systems
#ifndef BEACON_ENABLED
# define BEACON_ENABLED !HAL_MINIMIZE_FEATURES
Expand Down
8 changes: 8 additions & 0 deletions ArduCopter/crash_check.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,14 @@ void Copter::crash_check()
return;
}

#if MODE_AUTOROTATE_ENABLED == ENABLED
//return immediately if in autorotation mode
if (control_mode == Mode::Number::AUTOROTATE) {
crash_counter = 0;
return;
}
#endif

// vehicle not crashed if 1hz filtered acceleration is more than 3m/s (1G on Z-axis has been subtracted)
if (land_accel_ef_filter.get().length() >= CRASH_CHECK_ACCEL_MAX) {
crash_counter = 0;
Expand Down
34 changes: 34 additions & 0 deletions ArduCopter/heli.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -175,4 +175,38 @@ void Copter::heli_update_rotor_speed_targets()
rotor_runup_complete_last = motors->rotor_runup_complete();
}


// heli_update_autorotation - determines if aircraft is in autorotation and sets motors flag and switches
// to autorotation flight mode if manual collective is not being used.
void Copter::heli_update_autorotation()
MattKear marked this conversation as resolved.
Show resolved Hide resolved
{
MattKear marked this conversation as resolved.
Show resolved Hide resolved
#if MODE_AUTOROTATE_ENABLED == ENABLED
//set autonomous autorotation flight mode
if (!ap.land_complete && !motors->get_interlock() && !flightmode->has_manual_throttle() && g2.arot.is_enable()) {
heli_flags.in_autorotation = true;
set_mode(Mode::Number::AUTOROTATE, ModeReason::AUTOROTATION_START);
} else {
heli_flags.in_autorotation = false;
}

// sets autorotation flags through out libraries
heli_set_autorotation(heli_flags.in_autorotation);
if (!ap.land_complete && g2.arot.is_enable()) {
motors->set_enable_bailout(true);
} else {
motors->set_enable_bailout(false);
}
#else
heli_flags.in_autorotation = false;
motors->set_enable_bailout(false);
#endif
}

#if MODE_AUTOROTATE_ENABLED == ENABLED
// heli_set_autorotation - set the autorotation f`lag throughout libraries
void Copter::heli_set_autorotation(bool autorotation)
{
motors->set_in_autorotation(autorotation);
}
#endif
#endif // FRAME_CONFIG == HELI_FRAME
32 changes: 28 additions & 4 deletions ArduCopter/mode.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -163,6 +163,12 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode)
break;
#endif

#if MODE_AUTOROTATE_ENABLED == ENABLED
case Mode::Number::AUTOROTATE:
ret = &mode_autorotate;
break;
#endif

default:
break;
}
Expand Down Expand Up @@ -196,11 +202,29 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter a non-manual throttle mode if the
// rotor runup is not complete
if (!ignore_checks && !new_flightmode->has_manual_throttle() && (motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) {
gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
return false;
if (!ignore_checks &&
!new_flightmode->has_manual_throttle() &&
(motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) {
#if MODE_AUTOROTATE_ENABLED == ENABLED
//if the mode being exited is the autorotation mode allow mode change despite rotor not being at
//full speed. This will reduce altitude loss on bail-outs back to non-manual throttle modes
bool in_autorotation_check = (flightmode != &mode_autorotate || new_flightmode != &mode_autorotate);
#else
bool in_autorotation_check = false;
#endif

if (!in_autorotation_check) {
gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
return false;
}
}

#if MODE_AUTOROTATE_ENABLED == ENABLED
// If changing to autorotate flight mode from a non-manual throttle mode, store the previous flight mode
// to exit back to it when interlock is re-engaged
prev_control_mode = control_mode;
#endif
#endif

#if FRAME_CONFIG != HELI_FRAME
Expand Down
Loading