aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_IOMCU: moved protocol structures to a common header

Browse Source
This commit is contained in:
Andrew Tridgell 2018-05-11 10:37:01 +10:00
parent 89bfd7e850
commit 037a455784
5 changed files with 180 additions and 274 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -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)
{}

50
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -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 {

68
libraries/AP_IOMCU/iofirmware/iofirmware.cpp
View File

@ -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();

93
libraries/AP_IOMCU/iofirmware/iofirmware.h
View File

@ -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

147
libraries/AP_IOMCU/iofirmware/ioprotocol.h Normal file
View File

@ -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;
};