Merge pull request #77 from CarbonixUAV/feature/cherry-pick-carbopilt440

Feature/cherry pick carbopilt440

ArduPlane/quadplane.cpp

@@ -2431,10 +2431,11 @@ void QuadPlane::vtol_position_controller(void)
         const uint32_t min_airbrake_ms = 1000;
         if (poscontrol.get_state() == QPOS_AIRBRAKE &&
             poscontrol.time_since_state_start_ms() > min_airbrake_ms &&
-            (aspeed < aspeed_threshold ||
-             fabsf(degrees(closing_vel.angle(desired_closing_vel))) > 60 ||
-             closing_speed > MAX(desired_closing_speed*1.2, desired_closing_speed+2) ||
-             labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) > attitude_error_threshold_cd ||
+            (aspeed < aspeed_threshold || // too low airspeed
+             fabsf(degrees(closing_vel.angle(desired_closing_vel))) > 60 || // wrong direction
+             closing_speed > MAX(desired_closing_speed*1.2, desired_closing_speed+2) || // too fast
+             closing_speed < desired_closing_speed*0.5 || // too slow ground speed
+             labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) > attitude_error_threshold_cd || // bad attitude
              labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) > attitude_error_threshold_cd)) {
             gcs().send_text(MAV_SEVERITY_INFO,"VTOL position1 v=%.1f d=%.1f h=%.1f dc=%.1f",
                             (double)groundspeed,
@@ -2446,6 +2447,18 @@ void QuadPlane::vtol_position_controller(void)
 
             // switch to vfwd for throttle control
             vel_forward.integrator = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
+
+            // adjust the initial forward throttle based on our desired and actual closing speed
+            // this allows for significant initial forward throttle
+            // when we have a strong headwind, but low throttle in the usual case where
+            // we want to slow down ready for POSITION2
+            vel_forward.integrator = linear_interpolate(0, vel_forward.integrator,
+                                                        closing_speed,
+                                                        1.2*desired_closing_speed, 0.5*desired_closing_speed);
+
+            // limit our initial forward throttle in POSITION1 to be 0.5 of cruise throttle
+            vel_forward.integrator = constrain_float(vel_forward.integrator, 0, plane.aparm.throttle_cruise*0.5);
+
             vel_forward.last_ms = now_ms;
         }
 
@@ -3988,6 +4001,10 @@ Vector2f QuadPlane::landing_desired_closing_velocity()
     float tecs_land_airspeed = plane.TECS_controller.get_land_airspeed();
     if (is_positive(tecs_land_airspeed)) {
         land_speed = tecs_land_airspeed;
+    } else {
+        // use half way between min airspeed and cruise if
+        // TECS_LAND_AIRSPEED not set
+        land_speed = 0.5*(land_speed+plane.aparm.airspeed_min);
     }
     target_speed = MIN(target_speed, eas2tas * land_speed);
 
@@ -4029,12 +4046,16 @@ float QuadPlane::get_land_airspeed(void)
         return approach_speed;
     }
 
+    if (qstate == QPOS_AIRBRAKE) {
+        // during airbraking ask TECS to slow us to stall speed
+        return plane.aparm.airspeed_min;
+    }
+
     // calculate speed based on landing desired velocity
     Vector2f vel = landing_desired_closing_velocity();
-    const Vector3f wind = plane.ahrs.wind_estimate();
+    const Vector2f wind = plane.ahrs.wind_estimate().xy();
     const float eas2tas = plane.ahrs.get_EAS2TAS();
-    vel.x -= wind.x;
-    vel.y -= wind.y;
+    vel -= wind;
     vel /= eas2tas;
     return vel.length();
 }

Tools/AP_Periph/AP_Periph.cpp

@@ -386,7 +386,19 @@ void AP_Periph_FW::update()
         last_error_ms = now;
         can_printf("IERR 0x%x %u", unsigned(ierr.errors()), unsigned(ierr.last_error_line()));
     }
-
+//this will only monitor Arming signal
+#ifdef HAL_PERIPH_ARM_MONITORING_ENABLE
+    static uint32_t last_arm_check_ms;
+    if (now - last_arm_check_ms > g.disarm_delay){
+        last_arm_check_ms = now;
+        if(periph.arm_update_status){    
+            periph.arm_update_status = false;
+        }
+        else{
+            hal.util->set_soft_armed(UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_DISARMED);
+        }
+    }
+#endif
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && CH_DBG_ENABLE_STACK_CHECK == TRUE
     static uint32_t last_debug_ms;
     if ((g.debug&(1<<DEBUG_SHOW_STACK)) && now - last_debug_ms > 5000) {

Tools/AP_Periph/AP_Periph.h

@@ -216,6 +216,9 @@ class AP_Periph_FW {
     SRV_Channels servo_channels;
     bool rcout_has_new_data_to_update;
 
+    uint32_t last_esc_raw_command_ms;
+    uint8_t  last_esc_num_channels;
+
     void rcout_init();
     void rcout_init_1Hz();
     void rcout_esc(int16_t *rc, uint8_t num_channels);

Tools/AP_Periph/Parameters.cpp

@@ -359,6 +359,14 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @RebootRequired: True
     GSCALAR(esc_pwm_type, "ESC_PWM_TYPE",     0),
 
+    // @Param: ESC_CMD_TIMO
+    // @DisplayName: ESC Command Timeout
+    // @Description: This is the duration (ms) with which to hold the last driven ESC command before timing out and zeroing the ESC outputs. To disable zeroing of outputs in event of CAN loss, use 0. Use values greater than the expected duration between two CAN frames to ensure Periph is not starved of ESC Raw Commands.
+    // @Range: 0 10000
+    // @Units: ms
+    // @User: Advanced
+    GSCALAR(esc_command_timeout_ms, "ESC_CMD_TIMO",     200),
+
 #if HAL_WITH_ESC_TELEM && !HAL_GCS_ENABLED
     // @Param: ESC_TELEM_PORT
     // @DisplayName: ESC Telemetry Serial Port
@@ -449,6 +457,17 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @Group: EFI
     // @Path: ../libraries/AP_EFI/AP_EFI.cpp
     GOBJECT(efi, "EFI", AP_EFI),
+#endif 
+
+#ifdef HAL_PERIPH_ARM_MONITORING_ENABLE
+    // @Param: DISARM_DELAY
+    // @DisplayName: ARM Command Timeout
+    // @Description: This is the duration (ms) with which if not received a signal will disarm the system default to 2 sec as ARM packet is set to 1 Hz
+    // @Range: 0 10000
+    // @Units: ms
+    // @User: Advanced
+    GSCALAR(disarm_delay, "DISARM_DELAY", 2000),
+
 #endif
 
     AP_VAREND

Tools/AP_Periph/Parameters.h

@@ -59,6 +59,8 @@ class Parameters {
         k_param_efi,
         k_param_efi_port,
         k_param_efi_baudrate,
+        k_param_disarm_delay,
+        k_param_esc_command_timeout_ms,
     };
 
     AP_Int16 format_version;
@@ -115,6 +117,7 @@ class Parameters {
 
 #ifdef HAL_PERIPH_ENABLE_RC_OUT
     AP_Int8 esc_pwm_type;
+    AP_Int16 esc_command_timeout_ms;
 #if HAL_WITH_ESC_TELEM && !HAL_GCS_ENABLED
     AP_Int8 esc_telem_port;
 #endif

Tools/AP_Periph/can.cpp

@@ -762,6 +762,10 @@ static void handle_esc_rawcommand(CanardInstance* ins, CanardRxTransfer* transfe
         return;
     }
     periph.rcout_esc(cmd.cmd.data, cmd.cmd.len);
+
+    // Update internal copy for disabling output to ESC when CAN packets are lost
+    periph.last_esc_num_channels = cmd.cmd.len;
+    periph.last_esc_raw_command_ms = AP_HAL::millis();
 }
 
 static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer)

Tools/AP_Periph/rc_out.cpp

@@ -131,6 +131,19 @@ void AP_Periph_FW::rcout_handle_safety_state(uint8_t safety_state)
 
 void AP_Periph_FW::rcout_update()
 {
+     uint32_t now_ms = AP_HAL::millis();
+
+    const uint16_t esc_timeout_ms = g.esc_command_timeout_ms >= 0 ? g.esc_command_timeout_ms : 0; // Don't allow negative timeouts!
+    const bool has_esc_rawcommand_timed_out = esc_timeout_ms != 0 && ((now_ms - last_esc_raw_command_ms) >= esc_timeout_ms);
+    if (last_esc_num_channels > 0 && has_esc_rawcommand_timed_out) {
+        // If we've seen ESCs previously, and a timeout has occurred, then zero the outputs
+        int16_t esc_output[last_esc_num_channels] {};
+        rcout_esc(esc_output, last_esc_num_channels);
+
+        // register that the output has been changed
+        rcout_has_new_data_to_update = true;
+    }
+
     if (!rcout_has_new_data_to_update) {
         return;
     }
@@ -141,7 +154,6 @@ void AP_Periph_FW::rcout_update()
     SRV_Channels::output_ch_all();
     SRV_Channels::push();
 #if HAL_WITH_ESC_TELEM
-    uint32_t now_ms = AP_HAL::millis();
     if (now_ms - last_esc_telem_update_ms >= 20) {
         last_esc_telem_update_ms = now_ms;
         esc_telem_update();

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -456,10 +456,10 @@ void AP_UAVCAN::loop(void)
             continue;
         }
 
-        if (_SRV_armed) {
+        if (_SRV_armed_mask != 0 || _ESC_armed_mask != 0) {
             bool sent_servos = false;
 
-            if (_servo_bm > 0) {
+            if (_SRV_armed_mask != 0) {
                 // if we have any Servos in bitmask
                 uint32_t now = AP_HAL::native_micros();
                 const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get());
@@ -474,7 +474,7 @@ void AP_UAVCAN::loop(void)
             }
 
             // if we have any ESC's in bitmask
-            if (_esc_bm > 0 && !sent_servos) {
+            if (_ESC_armed_mask != 0 && !sent_servos) {
                 SRV_send_esc();
             }
 
@@ -525,7 +525,7 @@ void AP_UAVCAN::SRV_send_actuator(void)
              * physically possible throws at [-1:1] limits.
              */
 
-            if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _servo_bm)) {
+            if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _SRV_armed_mask)) {
                 cmd.actuator_id = starting_servo + 1;
 
                 if (option_is_set(Options::USE_ACTUATOR_PWM)) {
@@ -567,7 +567,7 @@ void AP_UAVCAN::SRV_send_esc(void)
 
     // find out how many esc we have enabled and if they are active at all
     for (uint8_t i = esc_offset; i < UAVCAN_SRV_NUMBER; i++) {
-        if ((((uint32_t) 1) << i) & _esc_bm) {
+        if ((((uint32_t) 1) << i) & _ESC_armed_mask) {
             max_esc_num = i + 1;
             if (_SRV_conf[i].esc_pending) {
                 active_esc_num++;
@@ -580,7 +580,7 @@ void AP_UAVCAN::SRV_send_esc(void)
         k = 0;
 
         for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) {
-            if ((((uint32_t) 1) << i) & _esc_bm) {
+            if ((((uint32_t) 1) << i) & _ESC_armed_mask) {
                 // TODO: ESC negative scaling for reverse thrust and reverse rotation
                 float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[i].pulse) + 1.0) / 2.0;
 
@@ -611,7 +611,22 @@ void AP_UAVCAN::SRV_push_servos()
         }
     }
 
-    _SRV_armed = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
+    uint16_t servo_armed_mask = _servo_bm;
+    uint16_t esc_armed_mask = _esc_bm;
+    const bool safety_off = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
+    if (!safety_off) {
+        AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton();
+        if (boardconfig != nullptr) {
+            const uint16_t safety_mask = boardconfig->get_safety_mask();
+            servo_armed_mask &= safety_mask;
+            esc_armed_mask &= safety_mask;
+        } else {
+            servo_armed_mask = 0;
+            esc_armed_mask = 0;
+        }
+    }
+    _SRV_armed_mask = servo_armed_mask;
+    _ESC_armed_mask = esc_armed_mask;
 }
 
 
@@ -836,7 +851,15 @@ void AP_UAVCAN::safety_state_send()
 
     { // handle SafetyState
         ardupilot::indication::SafetyState safety_msg;
-        switch (hal.util->safety_switch_state()) {
+        auto state = hal.util->safety_switch_state();
+        if (_SRV_armed_mask != 0 || _ESC_armed_mask != 0) {
+            // if we are outputting any servos or ESCs due to
+            // BRD_SAFETY_MASK then we need to advertise safety as
+            // off, this changes LEDs to indicate unsafe and allows
+            // AP_Periph ESCs and servos to run
+            state = AP_HAL::Util::SAFETY_ARMED;
+        }
+        switch (state) {
         case AP_HAL::Util::SAFETY_ARMED:
             safety_msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_OFF;
             break;

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -287,7 +287,12 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
         bool servo_pending;
     } _SRV_conf[UAVCAN_SRV_NUMBER];
 
-    uint8_t _SRV_armed;
+    uint32_t _esc_send_count;
+    uint32_t _srv_send_count;
+    uint32_t _fail_send_count;
+
+    uint16_t _SRV_armed_mask;
+    uint16_t _ESC_armed_mask;
     uint32_t _SRV_last_send_us;
     HAL_Semaphore SRV_sem;