Little bro 455

ArduCopter/Attitude.cpp

@@ -133,6 +133,10 @@ uint16_t Copter::get_pilot_speed_dn() const
     if (g2.pilot_speed_dn == 0) {
         return abs(g.pilot_speed_up);
     } else {
-        return abs(g2.pilot_speed_dn);
+        if (copter.baro_alt >= g2.land_alt_low) {
+        return abs(g2.pilot_speed_dn); 
+    } else {
+        return abs(g.land_speed);
     }
-}
+    }
+}

ArduCopter/Copter.cpp

@@ -242,9 +242,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
 #if AP_WINCH_ENABLED
     SCHED_TASK_CLASS(AP_Winch,             &copter.g2.winch,            update,          50,  50, 150),
 #endif
-#ifdef USERHOOK_FASTLOOP
-    SCHED_TASK(userhook_FastLoop,    100,     75, 153),
-#endif
+
+    SCHED_TASK(compass_rtl_run,      100,     75, 153),
+
 #ifdef USERHOOK_50HZLOOP
     SCHED_TASK(userhook_50Hz,         50,     75, 156),
 #endif
@@ -290,7 +290,6 @@ bool Copter::set_target_location(const Location& target_loc)
 
     return mode_guided.set_destination(target_loc);
 }
-
 // start takeoff to given altitude (for use by scripting)
 bool Copter::start_takeoff(const float alt)
 {
@@ -747,6 +746,8 @@ void Copter::three_hz_loop()
 
     // check if avoidance should be enabled based on alt
     low_alt_avoidance();
+        // update assigned functions and enable auxiliary servos
+    AP::srv().enable_aux_servos();
 }
 
 // ap_value calculates a 32-bit bitmask representing various pieces of
@@ -787,8 +788,8 @@ void Copter::one_hz_loop()
 #endif
     }
 
-    // update assigned functions and enable auxiliary servos
-    AP::srv().enable_aux_servos();
+    // rf amp and RC failsafe counter
+    rf_amp_power();
 
 #if HAL_LOGGING_ENABLED
     // log terrain data
@@ -960,6 +961,22 @@ bool Copter::get_rate_ef_targets(Vector3f& rate_ef_targets) const
     return true;
 }
 
+// Set during flight compass heading for non-GPS RTL
+void Copter::set_compass_rtl_heading()
+{
+    // if we are in Compass RTL we can't change home course.
+    if(flightmode->in_guided_mode()) {
+        return;
+    }
+
+    rtl_heading = (ahrs.yaw_sensor / 100) + 180;
+    if (rtl_heading >= 360) {
+         rtl_heading -= 360;
+    }
+    gcs().send_text(MAV_SEVERITY_CRITICAL, "%i Deg RTL Course", rtl_heading);
+}
+
+
 /*
   constructor for main Copter class
  */

ArduCopter/Copter.h

@@ -676,6 +676,7 @@ class Copter : public AP_Vehicle {
     void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
                              uint8_t &task_count,
                              uint32_t &log_bit) override;
+
 #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 #if MODE_GUIDED_ENABLED
     bool set_target_location(const Location& target_loc) override;
@@ -731,6 +732,7 @@ class Copter : public AP_Vehicle {
     bool get_wp_bearing_deg(float &bearing) const override;
     bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
     bool get_rate_ef_targets(Vector3f& rate_ef_targets) const override;
+    void set_compass_rtl_heading();
 
     // Attitude.cpp
     void update_throttle_hover();
@@ -831,6 +833,19 @@ class Copter : public AP_Vehicle {
     bool should_disarm_on_failsafe();
     void do_failsafe_action(FailsafeAction action, ModeReason reason);
     void announce_failsafe(const char *type, const char *action_undertaken=nullptr);
+    void rf_amp_power();
+    void compass_rtl_run();
+    bool ampswitch = false;
+    bool ampstate = false;
+    bool flte = false;
+    bool cr = false; //true when compass RTL call GNGP mode
+    uint32_t fltnorc;
+    uint32_t flth;
+    uint32_t flt;
+    uint32_t fltfs;
+    uint32_t fltrc;
+    int16_t rtl_heading;
+   // int8_t source_sw; // EKF3 source set 0-2
 
     // failsafe.cpp
     void failsafe_enable();

ArduCopter/Parameters.cpp

@@ -31,6 +31,28 @@
 #endif
 
 const AP_Param::Info Copter::var_info[] = {
+
+    // @Param: BRD_RF_AMP_CONTROL
+    // @DisplayName: RF Amp power 
+    // @Description: RF_Amp power supply control- 0 off , 1-switching, 2- reverse switching
+    // @Values: 0:non switch ,1:off/on control, 2: off/on inverted
+    // @User: Standard
+    GSCALAR(rf_amp_control, "RF_AMP_CONTROL", RF_AMP_CONTROL_DEFAULT),
+
+    // @Param: BRD_RF_AMP_ON_DIST
+    // @DisplayName: RF Amp power 
+    // @Description: RF_Amp power supply controll- 0 off , 1- supply of at home, on at 50 m distance and 15 m height
+    // @Values: 0:non switch ,1:off/on controll
+    // @User: Standard
+    GSCALAR(rf_amp_on_dist, "RF_AMP_ON_DIST", RF_AMP_ON_DEFAULT),
+
+    // @Param: BRD_RF_AMP_OFF_DIST
+    // @DisplayName: RF Amp power 
+    // @Description: RF_Amp power supply controll- 0 off , 1- supply of at home, on at 50 m distance and 15 m height
+    // @Values: 0:non switch ,1:off/on controll
+    // @User: Standard
+    GSCALAR(rf_amp_off_dist, "RF_AMP_OFF_DIST", RF_AMP_OFF_DEFAULT),
+
     // @Param: FORMAT_VERSION
     // @DisplayName: Eeprom format version number
     // @Description: This value is incremented when changes are made to the eeprom format

ArduCopter/Parameters.h

@@ -381,6 +381,12 @@ class Parameters {
         k_param_vehicle = 257, // vehicle common block of parameters
         k_param_throw_altitude_min,
         k_param_throw_altitude_max,
+        k_param_rf_amp_control,
+        k_param_rf_amp_on_dist,
+        k_param_rf_amp_off_dist,
+
+
+
 
         // the k_param_* space is 9-bits in size
         // 511: reserved
@@ -458,6 +464,9 @@ class Parameters {
     AP_Int8         fs_crash_check;
     AP_Float        fs_ekf_thresh;
     AP_Int16        gcs_pid_mask;
+    AP_Int8         rf_amp_control;
+    AP_Int32        rf_amp_on_dist;
+    AP_Int32        rf_amp_off_dist;
 
 #if MODE_THROW_ENABLED
     AP_Enum<ModeThrow::PreThrowMotorState>         throw_motor_start;

ArduCopter/RC_Channel.cpp

@@ -171,6 +171,8 @@ void RC_Channel_Copter::do_aux_function_change_mode(const Mode::Number mode,
 // do_aux_function - implement the function invoked by auxiliary switches
 bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch_flag)
 {
+    AP_NavEKF_Source::SourceSetSelection source_setted = AP_NavEKF_Source::SourceSetSelection::PRIMARY;
+
     switch(ch_option) {
         case AUX_FUNC::FLIP:
             // flip if switch is on, positive throttle and we're actually flying
@@ -202,7 +204,22 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
 
 #if MODE_RTL_ENABLED
         case AUX_FUNC::RTL:
-            do_aux_function_change_mode(Mode::Number::RTL, ch_flag);
+            switch(ch_flag) {
+              case AuxSwitchPos::LOW:
+                source_setted = AP_NavEKF_Source::SourceSetSelection::PRIMARY;
+                AP::ahrs().set_posvelyaw_source_set(source_setted); 
+                    copter.set_mode(Mode::Number::LAND, ModeReason::RC_COMMAND);
+                    break;
+                case AuxSwitchPos::MIDDLE:
+                source_setted = AP_NavEKF_Source::SourceSetSelection::PRIMARY;
+                AP::ahrs().set_posvelyaw_source_set(source_setted); 
+                copter.set_mode(Mode::Number::POSHOLD, ModeReason::RC_COMMAND);
+                    break;
+                case AuxSwitchPos::HIGH:
+                    source_setted = AP_NavEKF_Source::SourceSetSelection::PRIMARY;
+                    AP::ahrs().set_posvelyaw_source_set(source_setted); 
+                    copter.set_mode(Mode::Number::RTL, ModeReason::RC_COMMAND);
+                }
             break;
 #endif
 
@@ -338,21 +355,27 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
             }
             break;
 
-        case AUX_FUNC::PARACHUTE_3POS:
-            // Parachute disable, enable, release with 3 position switch
+       case AUX_FUNC::PARACHUTE_3POS:
+
             switch (ch_flag) {
-                case AuxSwitchPos::LOW:
-                    copter.parachute.enabled(false);
+                case AuxSwitchPos::LOW: 
+                    source_setted = AP_NavEKF_Source::SourceSetSelection::TERTIARY;
+                    AP::ahrs().set_posvelyaw_source_set(source_setted); 
+                    copter.set_mode(Mode::Number::LOITER, ModeReason::RC_COMMAND);
                     break;
                 case AuxSwitchPos::MIDDLE:
-                    copter.parachute.enabled(true);
+                    source_setted = AP_NavEKF_Source::SourceSetSelection::SECONDARY;
+                    AP::ahrs().set_posvelyaw_source_set(source_setted); 
+                    copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::RC_COMMAND);
                     break;
                 case AuxSwitchPos::HIGH:
-                    copter.parachute.enabled(true);
-                    copter.parachute_manual_release();
+                    source_setted = AP_NavEKF_Source::SourceSetSelection::PRIMARY;
+                    AP::ahrs().set_posvelyaw_source_set(source_setted); 
+                    copter.set_mode(Mode::Number::FLOWHOLD, ModeReason::RC_COMMAND);
                     break;
-            }
+                }
             break;
+
 #endif  // HAL_PARACHUTE_ENABLED
 
         case AUX_FUNC::ATTCON_FEEDFWD:
@@ -630,8 +653,7 @@ bool RC_Channel_Copter::do_aux_function(const AUX_FUNC ch_option, const AuxSwitc
 
         case AUX_FUNC::SIMPLE_HEADING_RESET:
             if (ch_flag == AuxSwitchPos::HIGH) {
-                copter.init_simple_bearing();
-                gcs().send_text(MAV_SEVERITY_INFO, "Simple heading reset");
+               copter.set_compass_rtl_heading();
             }
             break;
 

ArduCopter/commands.cpp

@@ -9,10 +9,8 @@ void Copter::update_home_from_EKF()
     }
 
     // special logic if home is set in-flight
-    if (motors->armed()) {
-        set_home_to_current_location_inflight();
-    } else {
-        // move home to current ekf location (this will set home_state to HOME_SET)
+    if (!motors->armed()) {
+           // move home to current ekf location (this will set home_state to HOME_SET)
         if (!set_home_to_current_location(false)) {
             // ignore failure
         }

ArduCopter/config.h

@@ -42,7 +42,7 @@
 #endif
 
 #ifndef ARMING_DELAY_SEC
-    # define ARMING_DELAY_SEC 2.0f
+    # define ARMING_DELAY_SEC 0.1f
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -211,7 +211,7 @@
 //////////////////////////////////////////////////////////////////////////////
 // Sport - fly vehicle in rate-controlled (earth-frame) mode
 #ifndef MODE_SPORT_ENABLED
-# define MODE_SPORT_ENABLED 0
+# define MODE_SPORT_ENABLED 1
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -344,16 +344,16 @@
  # define LAND_AIRMODE_DETECTOR_TRIGGER_SEC 3.0f    // number of seconds to detect a landing in air mode
 #endif
 #ifndef LAND_DETECTOR_MAYBE_TRIGGER_SEC
- # define LAND_DETECTOR_MAYBE_TRIGGER_SEC   0.2f    // number of seconds that means we might be landed (used to reset horizontal position targets to prevent tipping over)
+ # define LAND_DETECTOR_MAYBE_TRIGGER_SEC   0.3f    // number of seconds that means we might be landed (used to reset horizontal position targets to prevent tipping over)
 #endif
 #ifndef LAND_DETECTOR_ACCEL_LPF_CUTOFF
 # define LAND_DETECTOR_ACCEL_LPF_CUTOFF     1.0f    // frequency cutoff of land detector accelerometer filter
 #endif
 #ifndef LAND_DETECTOR_ACCEL_MAX
-# define LAND_DETECTOR_ACCEL_MAX            1.0f    // vehicle acceleration must be under 1m/s/s
+# define LAND_DETECTOR_ACCEL_MAX            2.0f    // vehicle acceleration must be under 1m/s/s
 #endif
 #ifndef LAND_DETECTOR_VEL_Z_MAX
-# define LAND_DETECTOR_VEL_Z_MAX              1.0f    // vehicle vertical velocity must be under 1m/s
+# define LAND_DETECTOR_VEL_Z_MAX              2.0f    // vehicle vertical velocity must be under 1m/s
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
@@ -507,6 +507,20 @@
 # define THROW_VERTICAL_SPEED   50.0f   // motors start when vehicle reaches this total 3D speed in cm/s
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+
+#ifndef RF_AMP_CONTROL_DEFAULT
+ # define RF_AMP_CONTROL_DEFAULT     1    // Enable/disable RF AMP power management
+#endif
+
+#ifndef RF_AMP_ON_DEFAULT
+ # define RF_AMP_ON_DEFAULT     200000    // RF Amp ON dist to home in cm
+#endif
+
+#ifndef RF_AMP_OFF_DEFAULT
+ # define RF_AMP_OFF_DEFAULT     150000    //  RF Amp OFF dist to home in cm
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // Logging control
 //

ArduCopter/ekf_check.cpp

@@ -166,6 +166,7 @@ bool Copter::ekf_over_threshold()
 // failsafe_ekf_event - perform ekf failsafe
 void Copter::failsafe_ekf_event()
 {
+     AP_NavEKF_Source::SourceSetSelection source_ = AP_NavEKF_Source::SourceSetSelection::SECONDARY;
     // EKF failsafe event has occurred
     failsafe.ekf = true;
     LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED);
@@ -189,10 +190,9 @@ void Copter::failsafe_ekf_event()
     // take action based on fs_ekf_action parameter
     switch (g.fs_ekf_action) {
         case FS_EKF_ACTION_ALTHOLD:
-            // AltHold
-            if (failsafe.radio || !set_mode(Mode::Number::ALT_HOLD, ModeReason::EKF_FAILSAFE)) {
-                set_mode_land_with_pause(ModeReason::EKF_FAILSAFE);
-            }
+            source_ = AP_NavEKF_Source::SourceSetSelection::SECONDARY;
+            AP::ahrs().set_posvelyaw_source_set(source_); 
+            set_mode(Mode::Number::ALT_HOLD, ModeReason::EKF_FAILSAFE);
             break;
         case FS_EKF_ACTION_LAND:
         case FS_EKF_ACTION_LAND_EVEN_STABILIZE:
@@ -217,7 +217,7 @@ void Copter::failsafe_ekf_off_event(void)
     failsafe.ekf = false;
     if (AP_Notify::flags.failsafe_ekf) {
         AP_Notify::flags.failsafe_ekf = false;
-        gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF Failsafe Cleared");
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF Good");
     }
     LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED);
 }
@@ -261,7 +261,7 @@ void Copter::check_vibration()
 {
     uint32_t now = AP_HAL::millis();
 
-    // assume checks will succeed
+    // assume checks will succeede
     bool innovation_checks_valid = true;
 
     // check if vertical velocity and position innovations are positive (NKF3.IVD & NKF3.IPD are both positive)
@@ -296,7 +296,7 @@ void Copter::check_vibration()
             vibration_check.high_vibes = true;
             pos_control->set_vibe_comp(true);
             LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED);
-            gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON");
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibes compensation ON");
         }
     } else {
         // initialise timer
@@ -311,7 +311,7 @@ void Copter::check_vibration()
             pos_control->set_vibe_comp(false);
             vibration_check.clear_ms = 0;
             LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED);
-            gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF");
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibes compensation OFF");
         }
     }