From 3428cbf9932bff33381cca41f88e8261e8567a01 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 3 Apr 2021 23:01:40 +0100 Subject: [PATCH] AP_HAL: add dshot command codes for beeps, leds and motor control allow more than one type of ESC for dshot commands add support for checks around active ESCs allow dshot alarm to be disabled allow prioritized dshot commands --- libraries/AP_HAL/RCOutput.h | 62 ++++++++++++++++++++++++++++++++ libraries/AP_HAL/Util.h | 8 ++++- libraries/AP_HAL/board/chibios.h | 8 +++++ libraries/AP_HAL/board/linux.h | 1 + libraries/AP_HAL/board/sitl.h | 1 + 5 files changed, 79 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL/RCOutput.h b/libraries/AP_HAL/RCOutput.h index 4decab6a87..61c2ffbeaf 100644 --- a/libraries/AP_HAL/RCOutput.h +++ b/libraries/AP_HAL/RCOutput.h @@ -57,6 +57,12 @@ public: */ virtual void set_reversible_mask(uint16_t chanmask) {} + /* + * mark the channels in chanmask as reversed. The chanmask passed is added (ORed) into + * any existing mask. + */ + virtual void set_reversed_mask(uint16_t chanmask) {} + /* * Delay subsequent calls to write() going to the underlying hardware in * order to group related writes together. When all the needed writes are @@ -187,6 +193,38 @@ public: MODE_NEOPIXEL, // same as MODE_PWM_DSHOT at 800kHz but it's an LED MODE_PROFILED, // same as MODE_PWM_DSHOT using separate clock and data }; + + // https://github.com/bitdump/BLHeli/blob/master/BLHeli_32%20ARM/BLHeli_32%20Firmware%20specs/Digital_Cmd_Spec.txt + enum BLHeliDshotCommand : uint8_t { + DSHOT_RESET = 0, + DSHOT_BEEP1 = 1, + DSHOT_BEEP2 = 2, + DSHOT_BEEP3 = 3, + DSHOT_BEEP4 = 4, + DSHOT_BEEP5 = 5, + DSHOT_ESC_INFO = 6, + DSHOT_ROTATE = 7, + DSHOT_ROTATE_ALTERNATE = 8, + DSHOT_3D_OFF = 9, + DSHOT_3D_ON = 10, + DSHOT_SAVE = 12, + DSHOT_NORMAL = 20, + DSHOT_REVERSE = 21, + DSHOT_LED0_ON = 22, + DSHOT_LED1_ON = 23, + DSHOT_LED2_ON = 24, + DSHOT_LED3_ON = 25, + DSHOT_LED0_OFF = 26, + DSHOT_LED1_OFF = 27, + DSHOT_LED2_OFF = 28, + DSHOT_LED3_OFF = 29, + }; + + enum DshotEscType { + DSHOT_ESC_NONE = 0, + DSHOT_ESC_BLHELI = 1 + }; + virtual void set_output_mode(uint16_t mask, enum output_mode mode) {} /* @@ -211,11 +249,35 @@ public: */ virtual void set_bidir_dshot_mask(uint16_t mask) {} + /* + mark escs as active for the purpose of sending dshot commands + */ + virtual void set_active_escs_mask(uint16_t mask) {} + /* Set the dshot rate as a multiple of the loop rate */ virtual void set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz) {} + /* + Set the dshot ESC type + */ + virtual void set_dshot_esc_type(DshotEscType esc_type) {} + + virtual DshotEscType get_dshot_esc_type() const { return DSHOT_ESC_NONE; } + + const static uint32_t ALL_CHANNELS = 255; + /* + Send a dshot command, if command timout is 0 then 10 commands are sent + chan is the servo channel to send the command to + */ + virtual void send_dshot_command(uint8_t command, uint8_t chan = ALL_CHANNELS, uint32_t command_timeout_ms = 0, uint16_t repeat_count = 10, bool priority = false) {} + + /* + If not already done flush any dshot commands still pending + */ + virtual bool prepare_for_arming() { return true; } + /* setup serial led output for a given channel number, with the given max number of LEDs in the chain. diff --git a/libraries/AP_HAL/Util.h b/libraries/AP_HAL/Util.h index 5a107f7409..a115ab692c 100644 --- a/libraries/AP_HAL/Util.h +++ b/libraries/AP_HAL/Util.h @@ -128,7 +128,13 @@ public: /* ToneAlarm Driver */ - virtual bool toneAlarm_init() { return false;} + enum ToneAlarmType { + ALARM_NONE=0, + ALARM_BUZZER=1<<0, + ALARM_DSHOT=1<<1 + }; + + virtual bool toneAlarm_init(uint8_t types) { return false;} virtual void toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) {} /* diff --git a/libraries/AP_HAL/board/chibios.h b/libraries/AP_HAL/board/chibios.h index 0a938acfa3..ceab85b2b1 100644 --- a/libraries/AP_HAL/board/chibios.h +++ b/libraries/AP_HAL/board/chibios.h @@ -108,6 +108,14 @@ #define HAL_SUPPORT_RCOUT_SERIAL !defined(HAL_BUILD_AP_PERIPH) #endif +#ifndef HAL_DSHOT_ALARM +#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD) +#define HAL_DSHOT_ALARM 1 +#else +#define HAL_DSHOT_ALARM 0 +#endif +#endif + // by default assume first I2C bus is internal #ifndef HAL_I2C_INTERNAL_MASK #define HAL_I2C_INTERNAL_MASK 1 diff --git a/libraries/AP_HAL/board/linux.h b/libraries/AP_HAL/board/linux.h index 5beed26f75..7e32c2669f 100644 --- a/libraries/AP_HAL/board/linux.h +++ b/libraries/AP_HAL/board/linux.h @@ -7,6 +7,7 @@ #define HAL_OS_SOCKETS 1 #define HAL_STORAGE_SIZE 16384 #define HAL_STORAGE_SIZE_AVAILABLE HAL_STORAGE_SIZE +#define HAL_DSHOT_ALARM 0 // make sensor selection clearer #define PROBE_IMU_I2C(driver, bus, addr, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,GET_I2C_DEVICE(bus, addr),##args)) diff --git a/libraries/AP_HAL/board/sitl.h b/libraries/AP_HAL/board/sitl.h index f9692f3172..cf01739860 100644 --- a/libraries/AP_HAL/board/sitl.h +++ b/libraries/AP_HAL/board/sitl.h @@ -5,6 +5,7 @@ #define HAL_MEM_CLASS HAL_MEM_CLASS_1000 #define HAL_OS_POSIX_IO 1 #define HAL_OS_SOCKETS 1 +#define HAL_DSHOT_ALARM 0 #define AP_FLASHSTORAGE_TYPE 3