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

Copter: Added autorotation flight mode and support

This commit is contained in:
Gone4Dirt 2019-11-28 20:21:07 +00:00 committed by Randy Mackay
parent ef72d89d26
commit 36ed12073c
12 changed files with 542 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/Copter.cpp
View File

@ -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
@ -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

16
ArduCopter/Copter.h
View File

@ -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"
@ -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
@ -230,6 +235,7 @@ public:
friend class ModeSystemId;
friend class ModeThrow;
friend class ModeZigZag;
friend class ModeAutorotate;
Copter(void);
@ -477,6 +483,7 @@ private:
AC_PosControl *pos_control;
AC_WPNav *wp_nav;
AC_Loiter *loiter_nav;
#if MODE_CIRCLE_ENABLED == ENABLED
AC_Circle *circle_nav;
#endif
@ -575,6 +582,7 @@ private:
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;
@ -751,7 +759,10 @@ private:
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();
@ -980,6 +991,9 @@ private:
#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);

25
ArduCopter/Parameters.cpp
View File

@ -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
// @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),
@ -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),
@ -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
};
@ -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
{
AP_Param::setup_object_defaults(this, var_info);
}

5
ArduCopter/Parameters.h
View File

@ -615,6 +615,11 @@ public:
// 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
ArduCopter/config.h
View File

@ -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

8
ArduCopter/crash_check.cpp
View File

@ -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;

34
ArduCopter/heli.cpp
View File

@ -180,4 +180,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()
{
#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
ArduCopter/mode.cpp
View File

@ -165,6 +165,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;
}
@ -198,11 +204,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

84
ArduCopter/mode.h
View File

@ -1,7 +1,6 @@
#pragma once
#include "Copter.h"
class Parameters;
class ParametersG2;
@ -36,6 +35,7 @@ public:
FOLLOW = 23, // follow attempts to follow another vehicle or ground station
ZIGZAG = 24, // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
SYSTEMID = 25, // System ID mode produces automated system identification signals in the controllers
AUTOROTATE = 26, // Autonomous autorotation
};
// constructor
@ -1348,7 +1348,7 @@ class ModeZigZag : public Mode {
public:
// inherit constructor
// Inherit constructor
using Mode::Mode;
bool init(bool ignore_checks) override;
@ -1388,3 +1388,83 @@ private:
uint32_t reach_wp_time_ms = 0; // time since vehicle reached destination (or zero if not yet reached)
};
#if MODE_AUTOROTATE_ENABLED == ENABLED
class ModeAutorotate : public Mode {
public:
// inherit constructor
using Mode::Mode;
bool init(bool ignore_checks) override;
void run() override;
bool is_autopilot() const override { return true; }
bool requires_GPS() const override { return false; }
bool has_manual_throttle() const override { return false; }
bool allows_arming(bool from_gcs) const override { return false; };
static const struct AP_Param::GroupInfo var_info[];
protected:
const char *name() const override { return "AUTOROTATE"; }
const char *name4() const override { return "AROT"; }
private:
// --- Internal variables ---
float _initial_rpm; // Head speed recorded at initiation of flight mode (RPM)
float _target_head_speed; // The terget head main rotor head speed. Normalised by main rotor set point
float _fwd_speed_target; // Target forward speed (cm/s)
float _desired_v_z; // Desired vertical
int32_t _pitch_target; // Target pitch attitude to pass to attitude controller
float _collective_aggression; // The 'aggresiveness' of collective appliction
float _z_touch_down_start; // The height in cm that the touch down phase began
float _t_touch_down_initiate; // The time in ms that the touch down phase began
float now; // Current time in millis
float _entry_time_start; // Time remaining until entry phase moves on to glide phase
float _hs_decay; // The head accerleration during the entry phase
float _bail_time; // Timer for exiting the bail out phase (s)
float _bail_time_start; // Time at start of bail out
float _des_z; // Desired vertical position
float _target_climb_rate_adjust;// Target vertical acceleration used during bail out phase
float _target_pitch_adjust; // Target pitch rate used during bail out phase
uint16_t log_counter; // Used to reduce the data flash logging rate
enum class Autorotation_Phase {
ENTRY,
SS_GLIDE,
FLARE,
TOUCH_DOWN,
BAIL_OUT } phase_switch;
enum class Navigation_Decision {
USER_CONTROL_STABILISED,
STRAIGHT_AHEAD,
INTO_WIND,
NEAREST_RALLY} nav_pos_switch;
// --- Internal flags ---
struct controller_flags {
bool entry_initial : 1;
bool ss_glide_initial : 1;
bool flare_initial : 1;
bool touch_down_initial : 1;
bool straight_ahead_initial : 1;
bool level_initial : 1;
bool break_initial : 1;
bool bail_out_initial : 1;
bool bad_rpm : 1;
} _flags;
struct message_flags {
bool bad_rpm : 1;
} _msg_flags;
//--- Internal functions ---
void warning_message(uint8_t message_n); //Handles output messages to the terminal
};
#endif

325
ArduCopter/mode_autorotate.cpp Normal file
View File

@ -0,0 +1,325 @@
/* -----------------------------------------------------------------------------------------
This is currently a SITL only function until the project is complete.
To trial this in SITL you will need to use Real Flight 8.
Instructions for how to set this up in SITL can be found here:
https://discuss.ardupilot.org/t/autonomous-autorotation-gsoc-project-blog/42139
-----------------------------------------------------------------------------------------*/
#include "Copter.h"
#include <AC_Autorotation/AC_Autorotation.h>
#include "mode.h"
#include <utility>
#if MODE_AUTOROTATE_ENABLED == ENABLED
#define AUTOROTATE_ENTRY_TIME 2.0f // (s) number of seconds that the entry phase operates for
#define BAILOUT_MOTOR_RAMP_TIME 1.0f // (s) time set on bailout ramp up timer for motors - See AC_MotorsHeli_Single
#define HEAD_SPEED_TARGET_RATIO 1.0f // Normalised target main rotor head speed (unit: -)
bool ModeAutorotate::init(bool ignore_checks)
{
#if FRAME_CONFIG != HELI_FRAME
// Only allow trad heli to use autorotation mode
return false;
#endif
// Check that mode is enabled
if (!g2.arot.is_enable()) {
gcs().send_text(MAV_SEVERITY_INFO, "Autorot Mode Not Enabled");
return false;
}
// Check that interlock is disengaged
if (motors->get_interlock()) {
gcs().send_text(MAV_SEVERITY_INFO, "Autorot Mode Change Fail: Interlock Engaged");
return false;
}
// Initialise controllers
// This must be done before RPM value is fetched
g2.arot.init_hs_controller();
g2.arot.init_fwd_spd_controller();
// Retrive rpm and start rpm sensor health checks
_initial_rpm = g2.arot.get_rpm(true);
// Display message
gcs().send_text(MAV_SEVERITY_INFO, "Autorotation initiated");
// Set all inial flags to on
_flags.entry_initial = 1;
_flags.ss_glide_initial = 1;
_flags.flare_initial = 1;
_flags.touch_down_initial = 1;
_flags.level_initial = 1;
_flags.break_initial = 1;
_flags.straight_ahead_initial = 1;
_flags.bail_out_initial = 1;
_msg_flags.bad_rpm = true;
// Setting default starting switches
phase_switch = Autorotation_Phase::ENTRY;
// Set entry timer
_entry_time_start = millis();
// The decay rate to reduce the head speed from the current to the target
_hs_decay = ((_initial_rpm/g2.arot.get_hs_set_point()) - HEAD_SPEED_TARGET_RATIO) / AUTOROTATE_ENTRY_TIME;
return true;
}
void ModeAutorotate::run()
{
// Check if interlock becomes engaged
if (motors->get_interlock() && !copter.ap.land_complete) {
phase_switch = Autorotation_Phase::BAIL_OUT;
} else if (motors->get_interlock() && copter.ap.land_complete) {
// Aircraft is landed and no need to bail out
set_mode(copter.prev_control_mode, ModeReason::AUTOROTATION_BAILOUT);
}
// Current time
now = millis(); //milliseconds
// Initialise internal variables
float curr_vel_z = inertial_nav.get_velocity().z; // Current vertical descent
//----------------------------------------------------------------
// State machine logic
//----------------------------------------------------------------
// Setting default phase switch positions
nav_pos_switch = Navigation_Decision::USER_CONTROL_STABILISED;
// Timer from entry phase to progress to glide phase
if (phase_switch == Autorotation_Phase::ENTRY){
if ((now - _entry_time_start)/1000.0f > AUTOROTATE_ENTRY_TIME) {
// Flight phase can be progressed to steady state glide
phase_switch = Autorotation_Phase::SS_GLIDE;
}
}
//----------------------------------------------------------------
// State machine actions
//----------------------------------------------------------------
switch (phase_switch) {
case Autorotation_Phase::ENTRY:
{
// Entry phase functions to be run only once
if (_flags.entry_initial == 1) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase");
#endif
// Set following trim low pass cut off frequency
g2.arot.set_col_cutoff_freq(g2.arot.get_col_entry_freq());
// Target head speed is set to rpm at initiation to prevent unwanted changes in attitude
_target_head_speed = _initial_rpm/g2.arot.get_hs_set_point();
// Set desired forward speed target
g2.arot.set_desired_fwd_speed();
// Prevent running the initial entry functions again
_flags.entry_initial = 0;
}
// Slowly change the target head speed until the target head speed matches the parameter defined value
if (g2.arot.get_rpm() > HEAD_SPEED_TARGET_RATIO*1.005f || g2.arot.get_rpm() < HEAD_SPEED_TARGET_RATIO*0.995f) {
_target_head_speed -= _hs_decay*G_Dt;
} else {
_target_head_speed = HEAD_SPEED_TARGET_RATIO;
}
// Set target head speed in head speed controller
g2.arot.set_target_head_speed(_target_head_speed);
// Run airspeed/attitude controller
g2.arot.set_dt(G_Dt);
g2.arot.update_forward_speed_controller();
// Retrieve pitch target
_pitch_target = g2.arot.get_pitch();
// Update controllers
_flags.bad_rpm = g2.arot.update_hs_glide_controller(G_Dt); //run head speed/ collective controller
break;
}
case Autorotation_Phase::SS_GLIDE:
{
// Steady state glide functions to be run only once
if (_flags.ss_glide_initial == 1) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs().send_text(MAV_SEVERITY_INFO, "SS Glide Phase");
#endif
// Set following trim low pass cut off frequency
g2.arot.set_col_cutoff_freq(g2.arot.get_col_glide_freq());
// Set desired forward speed target
g2.arot.set_desired_fwd_speed();
// Set target head speed in head speed controller
_target_head_speed = HEAD_SPEED_TARGET_RATIO; //Ensure target hs is set to glide incase hs hasent reached target for glide
g2.arot.set_target_head_speed(_target_head_speed);
// Prevent running the initial glide functions again
_flags.ss_glide_initial = 0;
}
// Run airspeed/attitude controller
g2.arot.set_dt(G_Dt);
g2.arot.update_forward_speed_controller();
// Retrieve pitch target
_pitch_target = g2.arot.get_pitch();
// Update head speed/ collective controller
_flags.bad_rpm = g2.arot.update_hs_glide_controller(G_Dt);
// Attitude controller is updated in navigation switch-case statements
break;
}
case Autorotation_Phase::FLARE:
case Autorotation_Phase::TOUCH_DOWN:
{
break;
}
case Autorotation_Phase::BAIL_OUT:
{
if (_flags.bail_out_initial == 1) {
// Functions and settings to be done once are done here.
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation");
#endif
// Set bail out timer remaining equal to the paramter value, bailout time
// cannot be less than the motor spool-up time: BAILOUT_MOTOR_RAMP_TIME.
_bail_time = MAX(g2.arot.get_bail_time(),BAILOUT_MOTOR_RAMP_TIME+0.1f);
// Set bail out start time
_bail_time_start = now;
// Set initial target vertical speed
_desired_v_z = curr_vel_z;
// Initialise position and desired velocity
if (!pos_control->is_active_z()) {
pos_control->relax_alt_hold_controllers(g2.arot.get_last_collective());
}
// Get pilot parameter limits
const float pilot_spd_dn = -get_pilot_speed_dn();
const float pilot_spd_up = g.pilot_speed_up;
// Set speed limit
pos_control->set_max_speed_z(curr_vel_z, pilot_spd_up);
float pilot_des_v_z = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
pilot_des_v_z = constrain_float(pilot_des_v_z, pilot_spd_dn, pilot_spd_up);
// Calculate target climb rate adjustment to transition from bail out decent speed to requested climb rate on stick.
_target_climb_rate_adjust = (curr_vel_z - pilot_des_v_z)/(_bail_time - BAILOUT_MOTOR_RAMP_TIME); //accounting for 0.5s motor spool time
// Calculate pitch target adjustment rate to return to level
_target_pitch_adjust = _pitch_target/_bail_time;
// Set acceleration limit
pos_control->set_max_accel_z(abs(_target_climb_rate_adjust));
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
_flags.bail_out_initial = 0;
}
if ((now - _bail_time_start)/1000.0f >= BAILOUT_MOTOR_RAMP_TIME) {
// Update desired vertical speed and pitch target after the bailout motor ramp timer has completed
_desired_v_z -= _target_climb_rate_adjust*G_Dt;
_pitch_target -= _target_pitch_adjust*G_Dt;
}
// Set position controller
pos_control->set_alt_target_from_climb_rate(_desired_v_z, G_Dt, false);
// Update controllers
pos_control->update_z_controller();
if ((now - _bail_time_start)/1000.0f >= _bail_time) {
// Bail out timer complete. Change flight mode. Do not revert back to auto. Prevent aircraft
// from continuing mission and potentially flying further away after a power failure.
if (copter.prev_control_mode == Mode::Number::AUTO) {
set_mode(Mode::Number::ALT_HOLD, ModeReason::AUTOROTATION_BAILOUT);
} else {
set_mode(copter.prev_control_mode, ModeReason::AUTOROTATION_BAILOUT);
}
}
break;
}
}
switch (nav_pos_switch) {
case Navigation_Decision::USER_CONTROL_STABILISED:
{
// Operator is in control of roll and yaw. Controls act as if in stabilise flight mode. Pitch
// is controlled by speed-height controller.
float pilot_roll, pilot_pitch;
get_pilot_desired_lean_angles(pilot_roll, pilot_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
// Get pilot's desired yaw rate
float pilot_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// Pitch target is calculated in autorotation phase switch above
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pilot_roll, _pitch_target, pilot_yaw_rate);
break;
}
case Navigation_Decision::STRAIGHT_AHEAD:
case Navigation_Decision::INTO_WIND:
case Navigation_Decision::NEAREST_RALLY:
{
break;
}
}
// Output warning messaged if rpm signal is bad
if (_flags.bad_rpm) {
warning_message(1);
}
} // End function run()
void ModeAutorotate::warning_message(uint8_t message_n)
{
switch (message_n) {
case 1:
{
if (_msg_flags.bad_rpm) {
// Bad rpm sensor health.
gcs().send_text(MAV_SEVERITY_INFO, "Warning: Poor RPM Sensor Health");
gcs().send_text(MAV_SEVERITY_INFO, "Action: Minimum Collective Applied");
_msg_flags.bad_rpm = false;
}
break;
}
}
}
#endif

0
ArduCopter/system.cpp Executable file → Normal file
View File

1
ArduCopter/wscript
View File

@ -11,6 +11,7 @@ def build(bld):
'AC_InputManager',
'AC_PrecLand',
'AC_Sprayer',
'AC_Autorotation',
'AC_WPNav',
'AP_Camera',
'AP_IRLock',