From 037a45578467d30b7c5add5f6dfbd7089c691905 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 11 May 2018 10:37:01 +1000 Subject: [PATCH] AP_IOMCU: moved protocol structures to a common header --- libraries/AP_IOMCU/AP_IOMCU.cpp | 96 ------------ libraries/AP_IOMCU/AP_IOMCU.h | 50 +------ libraries/AP_IOMCU/iofirmware/iofirmware.cpp | 68 ++------- libraries/AP_IOMCU/iofirmware/iofirmware.h | 93 +++--------- libraries/AP_IOMCU/iofirmware/ioprotocol.h | 147 +++++++++++++++++++ 5 files changed, 180 insertions(+), 274 deletions(-) create mode 100644 libraries/AP_IOMCU/iofirmware/ioprotocol.h diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index 6e40433aea..fb1ee0395f 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -17,102 +17,6 @@ extern const AP_HAL::HAL &hal; -#define PKT_MAX_REGS 32 - -//#define IOMCU_DEBUG - -struct PACKED IOPacket { - uint8_t count:6; - uint8_t code:2; - uint8_t crc; - uint8_t page; - uint8_t offset; - uint16_t regs[PKT_MAX_REGS]; - - // get packet size in bytes - uint8_t get_size(void) const { - return count*2 + 4; - } -}; - -/* - values for pkt.code - */ -enum iocode { - // read types - CODE_READ = 0, - CODE_WRITE = 1, - - // reply codes - CODE_SUCCESS = 0, - CODE_CORRUPT = 1, - CODE_ERROR = 2 -}; - -// IO pages -enum iopage { - PAGE_CONFIG = 0, - PAGE_STATUS = 1, - PAGE_ACTUATORS = 2, - PAGE_SERVOS = 3, - PAGE_RAW_RCIN = 4, - PAGE_RCIN = 5, - PAGE_RAW_ADC = 6, - PAGE_PWM_INFO = 7, - PAGE_SETUP = 50, - PAGE_DIRECT_PWM = 54, - PAGE_FAILSAFE_PWM = 55, - PAGE_DISARMED_PWM = 108, -}; - -// pending IO events to send, used as an event mask -enum ioevents { - IOEVENT_INIT=1, - IOEVENT_SEND_PWM_OUT, - IOEVENT_SET_DISARMED_PWM, - IOEVENT_SET_FAILSAFE_PWM, - IOEVENT_FORCE_SAFETY_OFF, - IOEVENT_FORCE_SAFETY_ON, - IOEVENT_SET_ONESHOT_ON, - IOEVENT_SET_RATES, - IOEVENT_GET_RCIN, - IOEVENT_ENABLE_SBUS, - IOEVENT_SET_HEATER_TARGET, - IOEVENT_SET_DEFAULT_RATE, - IOEVENT_SET_SAFETY_MASK, -}; - -// setup page registers -#define PAGE_REG_SETUP_FEATURES 0 -#define P_SETUP_FEATURES_SBUS1_OUT 1 -#define P_SETUP_FEATURES_SBUS2_OUT 2 -#define P_SETUP_FEATURES_PWM_RSSI 4 -#define P_SETUP_FEATURES_ADC_RSSI 8 -#define P_SETUP_FEATURES_ONESHOT 16 - -#define PAGE_REG_SETUP_ARMING 1 -#define P_SETUP_ARMING_IO_ARM_OK (1<<0) -#define P_SETUP_ARMING_FMU_ARMED (1<<1) -#define P_SETUP_ARMING_RC_HANDLING_DISABLED (1<<6) -#define P_SETUP_ARMING_SAFETY_DISABLE_ON (1 << 11) // disable use of safety button for safety off->on -#define P_SETUP_ARMING_SAFETY_DISABLE_OFF (1 << 12) // disable use of safety button for safety on->off - -#define PAGE_REG_SETUP_PWM_RATE_MASK 2 -#define PAGE_REG_SETUP_DEFAULTRATE 3 -#define PAGE_REG_SETUP_ALTRATE 4 -#define PAGE_REG_SETUP_REBOOT_BL 10 -#define PAGE_REG_SETUP_CRC 11 -#define PAGE_REG_SETUP_SBUS_RATE 19 -#define PAGE_REG_SETUP_IGNORE_SAFETY 20 /* bitmask of surfaces to ignore the safety status */ -#define PAGE_REG_SETUP_HEATER_DUTY_CYCLE 21 - -// magic value for rebooting to bootloader -#define REBOOT_BL_MAGIC 14662 - -#define PAGE_REG_SETUP_FORCE_SAFETY_OFF 12 -#define PAGE_REG_SETUP_FORCE_SAFETY_ON 14 -#define FORCE_SAFETY_MAGIC 22027 - AP_IOMCU::AP_IOMCU(AP_HAL::UARTDriver &_uart) : uart(_uart) {} diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index 7aca6ead52..79666c10c5 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -10,8 +10,7 @@ #if HAL_WITH_IO_MCU #include "ch.h" - -#define IOMCU_MAX_CHANNELS 16 +#include "iofirmware/ioprotocol.h" class AP_IOMCU { public: @@ -135,53 +134,10 @@ private: void update_safety_options(void); // PAGE_STATUS values - struct PACKED { - uint16_t freemem; - uint16_t cpuload; - - // status flags - uint16_t flag_outputs_armed:1; - uint16_t flag_override:1; - uint16_t flag_rc_ok:1; - uint16_t flag_rc_ppm:1; - uint16_t flag_rc_dsm:1; - uint16_t flag_rc_sbus:1; - uint16_t flag_fmu_ok:1; - uint16_t flag_raw_pwm:1; - uint16_t flag_mixer_ok:1; - uint16_t flag_arm_sync:1; - uint16_t flag_init_ok:1; - uint16_t flag_failsafe:1; - uint16_t flag_safety_off:1; - uint16_t flag_fmu_initialised:1; - uint16_t flag_rc_st24:1; - uint16_t flag_rc_sumd_srxl:1; - - uint16_t alarms; - uint16_t vbatt; - uint16_t ibatt; - uint16_t vservo; - uint16_t vrssi; - uint16_t prssi; - } reg_status; + struct page_reg_status reg_status; // PAGE_RAW_RCIN values - struct PACKED { - uint16_t count; - uint16_t flags_frame_drop:1; - uint16_t flags_failsafe:1; - uint16_t flags_dsm11:1; - uint16_t flags_mapping_ok:1; - uint16_t flags_rc_ok:1; - uint16_t flags_unused:11; - uint16_t nrssi; - uint16_t data; - uint16_t frame_count; - uint16_t lost_frame_count; - uint16_t pwm[IOMCU_MAX_CHANNELS]; - uint16_t last_frame_count; - uint32_t last_input_us; - } rc_input; + struct page_rc_input rc_input; // output pwm values struct { diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp index 3ca2d7f425..4481f57a71 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp @@ -8,69 +8,17 @@ #include "hal.h" extern const AP_HAL::HAL &hal; -#define PKT_MAX_REGS 32 -AP_IOMCU_FW iomcu; -/* - values for pkt.code - */ -enum iocode { - // read types - CODE_READ = 0, - CODE_WRITE = 1, +static AP_IOMCU_FW iomcu; - // reply codes - CODE_SUCCESS = 0, - CODE_CORRUPT = 1, - CODE_ERROR = 2 -}; - -// IO pages -enum iopage { - PAGE_CONFIG = 0, - PAGE_STATUS = 1, - PAGE_ACTUATORS = 2, - PAGE_SERVOS = 3, - PAGE_RAW_RCIN = 4, - PAGE_RCIN = 5, - PAGE_RAW_ADC = 6, - PAGE_PWM_INFO = 7, - PAGE_SETUP = 50, - PAGE_DIRECT_PWM = 54, -}; - -// setup page registers -#define PAGE_REG_SETUP_FEATURES 0 -#define P_SETUP_FEATURES_SBUS1_OUT 1 -#define P_SETUP_FEATURES_SBUS2_OUT 2 -#define P_SETUP_FEATURES_PWM_RSSI 4 -#define P_SETUP_FEATURES_ADC_RSSI 8 -#define P_SETUP_FEATURES_ONESHOT 16 - -#define PAGE_REG_SETUP_ARMING 1 -#define P_SETUP_ARMING_IO_ARM_OK (1<<0) -#define P_SETUP_ARMING_FMU_ARMED (1<<1) -#define P_SETUP_ARMING_RC_HANDLING_DISABLED (1<<6) - -#define PAGE_REG_SETUP_PWM_RATE_MASK 2 -#define PAGE_REG_SETUP_DEFAULTRATE 3 -#define PAGE_REG_SETUP_ALTRATE 4 -#define PAGE_REG_SETUP_REBOOT_BL 10 -#define PAGE_REG_SETUP_SBUS_RATE 19 -#define PAGE_REG_SETUP_HEATER_DUTY_CYCLE 21 - -#define PAGE_REG_SETUP_FORCE_SAFETY_OFF 12 -#define PAGE_REG_SETUP_FORCE_SAFETY_ON 14 -#define FORCE_SAFETY_MAGIC 22027 -#define PX4IO_REBOOT_BL_MAGIC 14662 void setup(); void loop(); const AP_HAL::HAL& hal = AP_HAL::get_HAL(); -static uint32_t num_code_read = 0, num_bad_crc = 0, num_write_pkt = 0, num_unknown_pkt = 0; -static uint32_t num_idle_rx = 0, num_dma_complete_rx = 0, num_total_rx = 0, num_rx_error = 0; +static uint32_t num_code_read, num_bad_crc, num_write_pkt, num_unknown_pkt; +static uint32_t num_idle_rx, num_dma_complete_rx, num_total_rx, num_rx_error; -void dma_rx_end_cb(UARTDriver *uart) +static void dma_rx_end_cb(UARTDriver *uart) { osalSysLockFromISR(); uart->usart->CR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); @@ -101,7 +49,8 @@ void dma_rx_end_cb(UARTDriver *uart) osalSysUnlockFromISR(); } -void idle_rx_handler(UARTDriver *uart) { +static void idle_rx_handler(UARTDriver *uart) +{ volatile uint16_t sr = uart->usart->SR; if (sr & (USART_SR_LBD | USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ @@ -339,6 +288,7 @@ bool AP_IOMCU_FW::handle_code_write() break; case PAGE_REG_SETUP_FORCE_SAFETY_OFF: if (rx_io_packet.regs[0] == FORCE_SAFETY_MAGIC) { + hal.rcout->force_safety_off(); reg_status.flag_safety_off = true; } else { return false; @@ -346,6 +296,7 @@ bool AP_IOMCU_FW::handle_code_write() break; case PAGE_REG_SETUP_FORCE_SAFETY_ON: if (rx_io_packet.regs[0] == FORCE_SAFETY_MAGIC) { + hal.rcout->force_safety_on(); reg_status.flag_safety_off = false; } else { return false; @@ -400,7 +351,7 @@ bool AP_IOMCU_FW::handle_code_write() } // check the magic value - if (rx_io_packet.regs[0] != PX4IO_REBOOT_BL_MAGIC) { + if (rx_io_packet.regs[0] != REBOOT_BL_MAGIC) { return false; } schedule_reboot(100); @@ -464,4 +415,5 @@ void AP_IOMCU_FW::calculate_fw_crc(void) reg_setup.crc[1] = sum >> 16; } + AP_HAL_MAIN(); diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h index fdf09b1736..e3064c6dc4 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.h +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h @@ -2,28 +2,17 @@ #include "ch.h" +#include "ioprotocol.h" #define IOMCU_MAX_CHANNELS 16 -#define PKT_MAX_REGS 32 #define PWM_IGNORE_THIS_CHANNEL UINT16_MAX #define SERVO_COUNT 8 + class AP_IOMCU_FW { public: void process_io_packet(); - struct PACKED IOPacket { - uint8_t count:6; - uint8_t code:2; - uint8_t crc; - uint8_t page; - uint8_t offset; - uint16_t regs[PKT_MAX_REGS]; - - // get packet size in bytes - uint8_t get_size(void) const { - return count*2 + 4; - } - } rx_io_packet = {0}, tx_io_packet = {0}; + struct IOPacket rx_io_packet, tx_io_packet; void init(); void update(); @@ -37,89 +26,47 @@ private: bool handle_code_write(); bool handle_code_read(); void schedule_reboot(uint32_t time_ms); + struct PACKED { /* default to RSSI ADC functionality */ - uint16_t features = 0; - uint16_t arming = 0; - uint16_t pwm_rates = 0; + uint16_t features; + uint16_t arming; + uint16_t pwm_rates; uint16_t pwm_defaultrate = 50; uint16_t pwm_altrate = 200; - uint16_t relays_pad = 0; + uint16_t relays_pad; uint16_t vbatt_scale = 10000; uint16_t reserved1; uint16_t reserved2; - uint16_t set_debug = 0; - uint16_t reboot_bl = 0; - uint16_t crc[2] = {0}; + uint16_t set_debug; + uint16_t reboot_bl; + uint16_t crc[2]; uint16_t rc_thr_failsafe_us; uint16_t reserved3; - uint16_t pwm_reverse = 0; - uint16_t trim_roll = 0; - uint16_t trim_pitch = 0; - uint16_t trim_yaw = 0; + uint16_t pwm_reverse; + uint16_t trim_roll; + uint16_t trim_pitch; + uint16_t trim_yaw; uint16_t sbus_rate = 72; - uint16_t ignore_safety = 0; + uint16_t ignore_safety; uint16_t heater_duty_cycle = 0xFFFFU; uint16_t pwm_altclock = 1; } reg_setup; // PAGE_STATUS values - struct PACKED { - uint16_t freemem; - uint16_t cpuload; - - // status flags - uint16_t flag_outputs_armed:1; - uint16_t flag_override:1; - uint16_t flag_rc_ok:1; - uint16_t flag_rc_ppm:1; - uint16_t flag_rc_dsm:1; - uint16_t flag_rc_sbus:1; - uint16_t flag_fmu_ok:1; - uint16_t flag_raw_pwm:1; - uint16_t flag_mixer_ok:1; - uint16_t flag_arm_sync:1; - uint16_t flag_init_ok:1; - uint16_t flag_failsafe:1; - uint16_t flag_safety_off:1; - uint16_t flag_fmu_initialised:1; - uint16_t flag_rc_st24:1; - uint16_t flag_rc_sumd_srxl:1; - - uint16_t alarms; - uint16_t vbatt; - uint16_t ibatt; - uint16_t vservo; - uint16_t vrssi; - uint16_t prssi; - } reg_status = {0}; + struct page_reg_status reg_status; // PAGE_RAW_RCIN values - struct PACKED { - uint16_t count; - uint16_t flags_frame_drop:1; - uint16_t flags_failsafe:1; - uint16_t flags_dsm11:1; - uint16_t flags_mapping_ok:1; - uint16_t flags_rc_ok:1; - uint16_t flags_unused:11; - uint16_t nrssi; - uint16_t data; - uint16_t frame_count; - uint16_t lost_frame_count; - uint16_t pwm[IOMCU_MAX_CHANNELS]; - uint16_t last_frame_count; - uint32_t last_input_us; - } rc_input = {0}; + struct page_rc_input rc_input; // PAGE_SERVO values struct { - uint16_t pwm[IOMCU_MAX_CHANNELS] = {0}; + uint16_t pwm[IOMCU_MAX_CHANNELS]; } reg_servo; // PAGE_SERVO values struct { - uint16_t pwm[IOMCU_MAX_CHANNELS]= {0}; + uint16_t pwm[IOMCU_MAX_CHANNELS]; } reg_direct_pwm; // output rates diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h new file mode 100644 index 0000000000..cd3dc400a3 --- /dev/null +++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h @@ -0,0 +1,147 @@ +/* + common protocol definitions between AP_IOMCU and iofirmware + */ + +#define PKT_MAX_REGS 32 +#define IOMCU_MAX_CHANNELS 16 + +//#define IOMCU_DEBUG + +struct PACKED IOPacket { + uint8_t count:6; + uint8_t code:2; + uint8_t crc; + uint8_t page; + uint8_t offset; + uint16_t regs[PKT_MAX_REGS]; + + // get packet size in bytes + uint8_t get_size(void) const { + return count*2 + 4; + } +}; + +/* + values for pkt.code + */ +enum iocode { + // read types + CODE_READ = 0, + CODE_WRITE = 1, + + // reply codes + CODE_SUCCESS = 0, + CODE_CORRUPT = 1, + CODE_ERROR = 2 +}; + +// IO pages +enum iopage { + PAGE_CONFIG = 0, + PAGE_STATUS = 1, + PAGE_ACTUATORS = 2, + PAGE_SERVOS = 3, + PAGE_RAW_RCIN = 4, + PAGE_RCIN = 5, + PAGE_RAW_ADC = 6, + PAGE_PWM_INFO = 7, + PAGE_SETUP = 50, + PAGE_DIRECT_PWM = 54, + PAGE_FAILSAFE_PWM = 55, + PAGE_DISARMED_PWM = 108, +}; + +// pending IO events to send, used as an event mask +enum ioevents { + IOEVENT_INIT=1, + IOEVENT_SEND_PWM_OUT, + IOEVENT_SET_DISARMED_PWM, + IOEVENT_SET_FAILSAFE_PWM, + IOEVENT_FORCE_SAFETY_OFF, + IOEVENT_FORCE_SAFETY_ON, + IOEVENT_SET_ONESHOT_ON, + IOEVENT_SET_RATES, + IOEVENT_GET_RCIN, + IOEVENT_ENABLE_SBUS, + IOEVENT_SET_HEATER_TARGET, + IOEVENT_SET_DEFAULT_RATE, + IOEVENT_SET_SAFETY_MASK, +}; + +// setup page registers +#define PAGE_REG_SETUP_FEATURES 0 +#define P_SETUP_FEATURES_SBUS1_OUT 1 +#define P_SETUP_FEATURES_SBUS2_OUT 2 +#define P_SETUP_FEATURES_PWM_RSSI 4 +#define P_SETUP_FEATURES_ADC_RSSI 8 +#define P_SETUP_FEATURES_ONESHOT 16 + +#define PAGE_REG_SETUP_ARMING 1 +#define P_SETUP_ARMING_IO_ARM_OK (1<<0) +#define P_SETUP_ARMING_FMU_ARMED (1<<1) +#define P_SETUP_ARMING_RC_HANDLING_DISABLED (1<<6) +#define P_SETUP_ARMING_SAFETY_DISABLE_ON (1 << 11) // disable use of safety button for safety off->on +#define P_SETUP_ARMING_SAFETY_DISABLE_OFF (1 << 12) // disable use of safety button for safety on->off + +#define PAGE_REG_SETUP_PWM_RATE_MASK 2 +#define PAGE_REG_SETUP_DEFAULTRATE 3 +#define PAGE_REG_SETUP_ALTRATE 4 +#define PAGE_REG_SETUP_REBOOT_BL 10 +#define PAGE_REG_SETUP_CRC 11 +#define PAGE_REG_SETUP_SBUS_RATE 19 +#define PAGE_REG_SETUP_IGNORE_SAFETY 20 /* bitmask of surfaces to ignore the safety status */ +#define PAGE_REG_SETUP_HEATER_DUTY_CYCLE 21 + +// magic value for rebooting to bootloader +#define REBOOT_BL_MAGIC 14662 + +#define PAGE_REG_SETUP_FORCE_SAFETY_OFF 12 +#define PAGE_REG_SETUP_FORCE_SAFETY_ON 14 +#define FORCE_SAFETY_MAGIC 22027 + +struct PACKED page_reg_status { + uint16_t freemem; + uint16_t cpuload; + + // status flags + uint16_t flag_outputs_armed:1; + uint16_t flag_override:1; + uint16_t flag_rc_ok:1; + uint16_t flag_rc_ppm:1; + uint16_t flag_rc_dsm:1; + uint16_t flag_rc_sbus:1; + uint16_t flag_fmu_ok:1; + uint16_t flag_raw_pwm:1; + uint16_t flag_mixer_ok:1; + uint16_t flag_arm_sync:1; + uint16_t flag_init_ok:1; + uint16_t flag_failsafe:1; + uint16_t flag_safety_off:1; + uint16_t flag_fmu_initialised:1; + uint16_t flag_rc_st24:1; + uint16_t flag_rc_sumd_srxl:1; + + uint16_t alarms; + uint16_t vbatt; + uint16_t ibatt; + uint16_t vservo; + uint16_t vrssi; + uint16_t prssi; +}; + +struct PACKED page_rc_input { + uint16_t count; + uint16_t flags_frame_drop:1; + uint16_t flags_failsafe:1; + uint16_t flags_dsm11:1; + uint16_t flags_mapping_ok:1; + uint16_t flags_rc_ok:1; + uint16_t flags_unused:11; + uint16_t nrssi; + uint16_t data; + uint16_t frame_count; + uint16_t lost_frame_count; + uint16_t pwm[IOMCU_MAX_CHANNELS]; + uint16_t last_frame_count; + uint32_t last_input_us; +};