diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 420387768a..1d768e4df9 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -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 diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 6ca1515564..4a2816c761 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -84,6 +84,10 @@ #define MOTOR_CLASS AP_MotorsMulticopter #endif +#if MODE_AUTOROTATE_ENABLED == ENABLED + #include // Autorotation controllers +#endif + #include "RC_Channel.h" // RC Channel Library #include "GCS_Mavlink.h" @@ -95,6 +99,7 @@ #if BEACON_ENABLED == ENABLED #include #endif + #if AC_AVOID_ENABLED == ENABLED #include #endif @@ -230,6 +235,7 @@ public: friend class ModeSystemId; friend class ModeThrow; friend class ModeZigZag; + friend class ModeAutorotate; Copter(void); @@ -478,6 +484,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; @@ -752,7 +760,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); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index a8b98f8434..af3847f2b6 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -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); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 2df4d1970b..bc0dc53c6a 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -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[]; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 6023da6f90..ea280c80b0 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -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 diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 99bd79a4a1..4fe7ad1c93 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -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; diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index e773a3b784..31e23da4d7 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -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() +{ +#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 diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 5a16ada4db..6aee3c798d 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -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; } @@ -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 diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 1410046888..c3fbd60b30 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -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 @@ -1352,7 +1352,7 @@ class ModeZigZag : public Mode { public: - // inherit constructor + // Inherit constructor using Mode::Mode; bool init(bool ignore_checks) override; @@ -1392,3 +1392,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 \ No newline at end of file diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp new file mode 100644 index 0000000000..f51c61ffe1 --- /dev/null +++ b/ArduCopter/mode_autorotate.cpp @@ -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 +#include "mode.h" + +#include + +#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 diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp old mode 100755 new mode 100644 diff --git a/ArduCopter/wscript b/ArduCopter/wscript index 7f21d6669a..8d90ab7e33 100644 --- a/ArduCopter/wscript +++ b/ArduCopter/wscript @@ -11,6 +11,7 @@ def build(bld): 'AC_InputManager', 'AC_PrecLand', 'AC_Sprayer', + 'AC_Autorotation', 'AC_WPNav', 'AP_Camera', 'AP_IRLock',