Merge pull request #68 from CarbonixUAV/feature/VO1-247-merge-ottano-…

…imp-commits-aug

Feature/vo1 247 merge ottano imp commits aug

.gitignore

@@ -129,7 +129,6 @@ way.txt
 *.wbproj
 *.wbproj
 segv_*out
-/scripts/
 /repl/
 /Rover/scripts/
 /Rover/repl/

ArduPlane/ReleaseNotes.txt

@@ -1,3 +1,11 @@
+Release Volanti Carbopilot V4.3.4 23rd Aug 2023
+----------------------------------
+
+This Release includes
+- feature/VO1-247-merge-ottano-imp-commits-aug
+
+Happy Flying!
+
 Release Volanti Carbopilot V4.3.3 23rd Aug 2023
 ----------------------------------
 

ArduPlane/quadplane.cpp

@@ -2370,10 +2370,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,
@@ -2385,6 +2386,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;
         }
 
@@ -3835,6 +3848,20 @@ Vector2f QuadPlane::landing_desired_closing_velocity()
 
     // base target speed based on sqrt of distance
     float target_speed = safe_sqrt(2*transition_decel*dist);
+
+    // don't let the target speed go above landing approach speed
+    const float eas2tas = plane.ahrs.get_EAS2TAS();
+    float land_speed = plane.aparm.airspeed_cruise_cm * 0.01;
+    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);
+
     Vector2f target_speed_xy = diff_wp.normalized() * target_speed;
 
     return target_speed_xy;
@@ -3845,7 +3872,8 @@ Vector2f QuadPlane::landing_desired_closing_velocity()
 */
 float QuadPlane::get_land_airspeed(void)
 {
-    if (poscontrol.get_state() == QPOS_APPROACH ||
+    const auto qstate = poscontrol.get_state();
+    if (qstate == QPOS_APPROACH ||
         plane.control_mode == &plane.mode_rtl) {
         float land_airspeed = plane.TECS_controller.get_land_airspeed();
         if (!is_positive(land_airspeed)) {
@@ -3861,12 +3889,17 @@ float QuadPlane::get_land_airspeed(void)
                                            20, 60);
         return land_airspeed;
     }
-    Vector2f vel = landing_desired_closing_velocity();
 
+    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 Vector2f wind = plane.ahrs.wind_estimate().xy();
     const float eas2tas = plane.ahrs.get_EAS2TAS();
-    const Vector3f wind = plane.ahrs.wind_estimate();
-    vel.x -= wind.x;
-    vel.y -= wind.y;
+    vel -= wind;
     vel /= eas2tas;
     return vel.length();
 }

ArduPlane/version.h

@@ -6,7 +6,7 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "Volanti Carbopilot V4.3.3"
+#define THISFIRMWARE "Volanti Carbopilot V4.3.4"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 4,2,1,FIRMWARE_VERSION_TYPE_OFFICIAL

Tools/AP_Periph/version.h

@@ -3,7 +3,7 @@
 #ifndef __AP_PERIPH_FW_VERSION__
 #define __AP_PERIPH_FW_VERSION__
 
-#define THISFIRMWARE "Volanti Carbopilot V4.3.3"
+#define THISFIRMWARE "Volanti Carbopilot V4.3.4"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 1,3,0,FIRMWARE_VERSION_TYPE_DEV

libraries/AP_IOMCU/iofirmware/rc.cpp

@@ -61,6 +61,12 @@ static const SerialConfig dsm_cfg = {
     nullptr,  // ctx
 };
 
+static enum {
+    RC_SEARCHING,
+    RC_DSM_PORT,
+    RC_SBUS_PORT
+} rc_state;
+
 /*
   init rcin on DSM USART1
  */
@@ -95,24 +101,34 @@ void AP_IOMCU_FW::rcin_serial_update(void)
     uint8_t b[16];
     uint32_t n;
     uint32_t now = AP_HAL::millis();
+    auto &rc = AP::RC();
+
+    if (rc.should_search(now)) {
+        rc_state = RC_SEARCHING;
+    }
 
     // read from DSM port
-    if ((n = chnReadTimeout(&SD1, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
+    if ((rc_state == RC_SEARCHING || rc_state == RC_DSM_PORT) &&
+        (n = chnReadTimeout(&SD1, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
         n = MIN(n, sizeof(b));
         // don't mix two 115200 uarts
         if (sd3_config == 0) {
             rc_stats.num_dsm_bytes += n;
             for (uint8_t i=0; i<n; i++) {
-                if (AP::RC().process_byte(b[i], 115200)) {
+                if (rc.process_byte(b[i], 115200)) {
                     rc_stats.last_good_ms = now;
+                    if (!rc.should_search(now)) {
+                        rc_state = RC_DSM_PORT;
+                    }
                 }
             }
         }
         //BLUE_TOGGLE();
     }
 
     // read from SBUS port
-    if ((n = chnReadTimeout(&SD3, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
+    if ((rc_state == RC_SEARCHING || rc_state == RC_SBUS_PORT) &&
+        (n = chnReadTimeout(&SD3, b, sizeof(b), TIME_IMMEDIATE)) > 0) {
         eventflags_t flags;
         if (sd3_config == 0 && ((flags = chEvtGetAndClearFlags(&sd3_listener)) & SD_PARITY_ERROR)) {
             rc_stats.sbus_error = flags;
@@ -121,8 +137,11 @@ void AP_IOMCU_FW::rcin_serial_update(void)
             n = MIN(n, sizeof(b));
             rc_stats.num_sbus_bytes += n;
             for (uint8_t i=0; i<n; i++) {
-                if (AP::RC().process_byte(b[i], sd3_config==0?100000:115200)) {
+                if (rc.process_byte(b[i], sd3_config==0?100000:115200)) {
                     rc_stats.last_good_ms = now;
+                    if (!rc.should_search(now)) {
+                        rc_state = RC_SBUS_PORT;
+                    }
                 }
             }
         }
@@ -135,7 +154,8 @@ void AP_IOMCU_FW::rcin_serial_update(void)
       output
      */
     const bool sbus_out_enabled = (reg_setup.features & P_SETUP_FEATURES_SBUS1_OUT) != 0;
-    if (now - rc_stats.last_good_ms > 2000 && (sd3_config==1 || !sbus_out_enabled)) {
+    if (rc_state == RC_SEARCHING &&
+        now - rc_stats.last_good_ms > 2000 && (sd3_config==1 || !sbus_out_enabled)) {
         rc_stats.last_good_ms = now;
         sd3_config ^= 1;
         sdStop(&SD3);

libraries/AP_RCProtocol/AP_RCProtocol.cpp

@@ -214,7 +214,7 @@ bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
                 }
                 // stop decoding pulses to save CPU
                 hal.rcin->pulse_input_enable(false);
-                break;
+                return true;
             }
         }
     }

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -402,10 +402,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());
@@ -420,7 +420,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();
             }
 
@@ -469,7 +469,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;
 
                 // TODO: other types
@@ -513,7 +513,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++;
@@ -526,7 +526,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;
 
@@ -561,7 +561,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;
 }
 
 
@@ -786,7 +801,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

@@ -295,7 +295,8 @@ class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     uint32_t _srv_send_count;
     uint32_t _fail_send_count;
 
-    uint8_t _SRV_armed;
+    uint16_t _SRV_armed_mask;
+    uint16_t _ESC_armed_mask;
     uint32_t _SRV_last_send_us;
     HAL_Semaphore SRV_sem;