From 9def37562a5c48c619f3820ba1ca3ad8af2c75f1 Mon Sep 17 00:00:00 2001 From: Lokesh-Ramina Date: Tue, 21 Nov 2023 15:00:24 -0800 Subject: [PATCH 1/3] AP_Periph: Adds user configurable timeout for ARM and ESC pkt on CAN ESC_Timeout: Fixes a periph continuing to drive an ESC to an output when the esc_rawcommand packets are lost ARM Timeout: is changed to 2000 as it is now only scanning ARM signal which is arriving at 1Hz Parameters added : ESC timeout: Timeout defaults to 200ms. Timeout can be disabled by setting parameter to 0. Timeout should be configured to be able to handle at small number of missed packets. ARM _Monitoring: dropped to be also cleared at ESC signal received --- Tools/AP_Periph/AP_Periph.cpp | 14 +++++++++++++- Tools/AP_Periph/AP_Periph.h | 3 +++ Tools/AP_Periph/Parameters.cpp | 19 +++++++++++++++++++ Tools/AP_Periph/Parameters.h | 3 +++ Tools/AP_Periph/can.cpp | 4 ++++ Tools/AP_Periph/rc_out.cpp | 14 +++++++++++++- 6 files changed, 55 insertions(+), 2 deletions(-) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 4d228cdab3..2953f664ae 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/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< 5000) { diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 85f356659d..9388e67f5f 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/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); diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 394a77a6fe..981e86dd2d 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/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 diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 092b4106ad..19f990f962 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/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 diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index d7759bda54..4afcc10bff 100644 --- a/Tools/AP_Periph/can.cpp +++ b/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) diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp index 2b105184a6..f260744369 100644 --- a/Tools/AP_Periph/rc_out.cpp +++ b/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(); From 60363ed1368a7aafa897b6089dd93077b1e4d8a1 Mon Sep 17 00:00:00 2001 From: Lokesh-Ramina Date: Mon, 20 Nov 2023 17:07:40 -0800 Subject: [PATCH 2/3] AP_UAVCAN: allow BRD_SAFETY_MASK to work on CAN ESCs and servos this allows for testing of a fwd motor or control surfaces while not allowing for VTOL ESCs to run. This makes CAN actuators behave the same as direct PWM actuators --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 39 ++++++++++++++++++++++++------- libraries/AP_UAVCAN/AP_UAVCAN.h | 7 +++++- 2 files changed, 37 insertions(+), 9 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index baeabe9557..cbdbd8df77 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/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; diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 9085a76c5a..b64d9ad53a 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/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; From 82dbf51065d87f1e3eb9c2cbd785ba71a6cd76d1 Mon Sep 17 00:00:00 2001 From: Lokesh-Ramina Date: Mon, 20 Nov 2023 17:17:34 -0800 Subject: [PATCH 3/3] Plane: improved fwd throttle during VTOL landing this sets up the vwd integrator more reasonably when we are in POSITION1 stage of VTOL landing. We need to have enough throttle to cope with a headwind, but want it lower when we are at or above our target closing speed so can minimise the amount of pitch up This also makes the landing_desired_closing_velocity() consistent with the landing speed used in approach, using average of airspeed min and cruise speed if TECS_LAND_ARSPD is not set The target airspeed for TECS during airbraking is now set to ARSPD_FBW_MIN, on the basis we are trying to slow down to min speed, and we have VTOL support which should prevent a stall. To cope with a high headwind where ARSPD_FBW_MIN is below the headwind we now check for too low achieved closing speed and switch to POSITION1 which can use vfwd to get to the landing location --- ArduPlane/quadplane.cpp | 35 ++++++++++++++++++++++++++++------- 1 file changed, 28 insertions(+), 7 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index ebecaaff3d..8a561482bd 100644 --- a/ArduPlane/quadplane.cpp +++ b/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(); }