e6e0543b8a
implement dshot ESC telemetry add support for channel enablement/disablement add stack checks and reporting for MSP stack wait correct timeout in tickless mode ensure that dshot sees all pwm updates as the occur in order to maintain periodicity ensure dshot options are propagated on reset implement dshot commands ensure oneshot/125 and mode are setup correctly add instrumentation for process stack prevent illegal recursive locks ignore requests for dshot 600 add support for soft reboot of iomcu
348 lines
8.9 KiB
C++
348 lines
8.9 KiB
C++
/*
|
|
implement protocol for controlling an IO microcontroller
|
|
|
|
For bootstrapping this will initially implement the px4io protocol,
|
|
but will later move to an ArduPilot specific protocol
|
|
*/
|
|
#pragma once
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#if HAL_WITH_IO_MCU
|
|
|
|
#include "iofirmware/ioprotocol.h"
|
|
#include <AP_RCMapper/AP_RCMapper.h>
|
|
|
|
typedef uint32_t eventmask_t;
|
|
typedef struct ch_thread thread_t;
|
|
|
|
class AP_IOMCU {
|
|
public:
|
|
AP_IOMCU(AP_HAL::UARTDriver &uart);
|
|
|
|
void init(void);
|
|
|
|
// write to one channel
|
|
void write_channel(uint8_t chan, uint16_t pwm);
|
|
|
|
// read from one channel
|
|
uint16_t read_channel(uint8_t chan);
|
|
|
|
// cork output
|
|
void cork(void);
|
|
|
|
// push output
|
|
void push(void);
|
|
|
|
// set output frequency
|
|
void set_freq(uint16_t chmask, uint16_t freq);
|
|
|
|
// get output frequency
|
|
uint16_t get_freq(uint16_t chan);
|
|
|
|
// get state of safety switch
|
|
AP_HAL::Util::safety_state get_safety_switch_state(void) const;
|
|
|
|
// force safety on
|
|
bool force_safety_on(void);
|
|
|
|
// force safety off
|
|
void force_safety_off(void);
|
|
|
|
// set mask of channels that ignore safety state
|
|
void set_safety_mask(uint16_t chmask);
|
|
|
|
// set PWM of channels when in FMU failsafe
|
|
void set_failsafe_pwm(uint16_t chmask, uint16_t period_us);
|
|
|
|
/*
|
|
enable sbus output
|
|
*/
|
|
bool enable_sbus_out(uint16_t rate_hz);
|
|
|
|
/*
|
|
check for new RC input
|
|
*/
|
|
bool check_rcinput(uint32_t &last_frame_us, uint8_t &num_channels, uint16_t *channels, uint8_t max_channels);
|
|
|
|
// Do DSM receiver binding
|
|
void bind_dsm(uint8_t mode);
|
|
|
|
// get the name of the RC protocol
|
|
const char *get_rc_protocol(void);
|
|
|
|
// get receiver RSSI
|
|
int16_t get_RSSI(void) const {
|
|
return rc_input.rssi;
|
|
}
|
|
|
|
/*
|
|
get servo rail voltage adc counts
|
|
*/
|
|
uint16_t get_vservo_adc_count(void) const { return reg_status.vservo; }
|
|
|
|
/*
|
|
get rssi voltage adc counts
|
|
*/
|
|
uint16_t get_vrssi_adc_count(void) const { return reg_status.vrssi; }
|
|
|
|
// set target for IMU heater
|
|
void set_heater_duty_cycle(uint8_t duty_cycle);
|
|
|
|
// set default output rate
|
|
void set_default_rate(uint16_t rate_hz);
|
|
|
|
// set to oneshot mode
|
|
void set_oneshot_mode(void);
|
|
|
|
// set to brushed mode
|
|
void set_brushed_mode(void);
|
|
|
|
// set output mode
|
|
void set_output_mode(uint16_t mask, uint16_t mode);
|
|
|
|
#if HAL_DSHOT_ENABLED
|
|
// set dshot output period
|
|
void set_dshot_period(uint16_t period_us, uint8_t drate);
|
|
|
|
// set telem request mask
|
|
void set_telem_request_mask(uint32_t mask);
|
|
|
|
// send a dshot command
|
|
void send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms, uint16_t repeat_count, bool priority);
|
|
#endif
|
|
// setup channels
|
|
void enable_ch(uint8_t ch);
|
|
void disable_ch(uint8_t ch);
|
|
|
|
// check if IO is healthy
|
|
bool healthy(void);
|
|
|
|
// shutdown IO protocol (for reboot)
|
|
void shutdown();
|
|
|
|
void soft_reboot();
|
|
|
|
// setup for FMU failsafe mixing
|
|
bool setup_mixing(RCMapper *rcmap, int8_t override_chan,
|
|
float mixing_gain, uint16_t manual_rc_mask);
|
|
|
|
// Check if pin number is valid and configured for GPIO
|
|
bool valid_GPIO_pin(uint8_t pin) const;
|
|
|
|
// convert external pin numbers 101 to 108 to internal 0 to 7
|
|
bool convert_pin_number(uint8_t& pin) const;
|
|
|
|
// set GPIO mask of channels setup for output
|
|
void set_GPIO_mask(uint8_t mask);
|
|
|
|
// write to a output pin
|
|
void write_GPIO(uint8_t pin, bool value);
|
|
|
|
// toggle a output pin
|
|
void toggle_GPIO(uint8_t pin);
|
|
|
|
// channel group masks
|
|
const uint8_t ch_masks[3] = { 0x03,0x0C,0xF0 };
|
|
|
|
static AP_IOMCU *get_singleton(void) {
|
|
return singleton;
|
|
}
|
|
|
|
private:
|
|
AP_HAL::UARTDriver &uart;
|
|
|
|
void thread_main(void);
|
|
|
|
// read count 16 bit registers
|
|
bool read_registers(uint8_t page, uint8_t offset, uint8_t count, uint16_t *regs);
|
|
|
|
// write count 16 bit registers
|
|
bool write_registers(uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs);
|
|
|
|
// write a single register
|
|
bool write_register(uint8_t page, uint8_t offset, uint16_t v) {
|
|
return write_registers(page, offset, 1, &v);
|
|
}
|
|
|
|
// modify a single register
|
|
bool modify_register(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits);
|
|
|
|
// trigger an ioevent
|
|
void trigger_event(uint8_t event);
|
|
|
|
// IOMCU thread
|
|
thread_t *thread_ctx;
|
|
|
|
eventmask_t initial_event_mask;
|
|
|
|
// time when we last read various pages
|
|
uint32_t last_status_read_ms;
|
|
uint32_t last_rc_read_ms;
|
|
uint32_t last_servo_read_ms;
|
|
uint32_t last_safety_option_check_ms;
|
|
uint32_t last_reg_read_ms;
|
|
|
|
// last value of safety options
|
|
uint16_t last_safety_options = 0xFFFF;
|
|
|
|
// have we forced the safety off?
|
|
bool safety_forced_off;
|
|
|
|
// was safety off on last status?
|
|
bool last_safety_off;
|
|
|
|
void send_servo_out(void);
|
|
void read_rc_input(void);
|
|
void read_servo(void);
|
|
void read_status(void);
|
|
void discard_input(void);
|
|
void event_failed(uint32_t event_mask);
|
|
void update_safety_options(void);
|
|
void send_rc_protocols(void);
|
|
|
|
// CONFIG page
|
|
struct page_config config;
|
|
|
|
// PAGE_STATUS values
|
|
struct page_reg_status reg_status;
|
|
uint32_t last_log_ms;
|
|
|
|
// PAGE_RAW_RCIN values
|
|
struct page_rc_input rc_input;
|
|
uint32_t rc_last_input_ms;
|
|
|
|
// MIXER values
|
|
struct page_mixing mixing;
|
|
|
|
// output pwm values
|
|
struct {
|
|
uint8_t num_channels;
|
|
uint16_t pwm[IOMCU_MAX_CHANNELS];
|
|
uint16_t safety_mask;
|
|
uint16_t failsafe_pwm[IOMCU_MAX_CHANNELS];
|
|
uint8_t failsafe_pwm_set;
|
|
uint8_t failsafe_pwm_sent;
|
|
uint16_t channel_mask;
|
|
} pwm_out;
|
|
|
|
// read back pwm values
|
|
struct {
|
|
uint16_t pwm[IOMCU_MAX_CHANNELS];
|
|
} pwm_in;
|
|
|
|
// output rates
|
|
struct {
|
|
uint16_t freq;
|
|
uint16_t chmask;
|
|
uint16_t default_freq = 50;
|
|
uint16_t sbus_rate_hz;
|
|
bool oneshot_enabled;
|
|
bool brushed_enabled;
|
|
} rate;
|
|
|
|
struct {
|
|
uint16_t period_us;
|
|
uint16_t rate;
|
|
} dshot_rate;
|
|
|
|
struct page_dshot dshot;
|
|
|
|
struct page_GPIO GPIO;
|
|
// output mode values
|
|
struct {
|
|
uint16_t mask;
|
|
uint16_t mode;
|
|
} mode_out;
|
|
|
|
// IMU heater duty cycle
|
|
uint8_t heater_duty_cycle;
|
|
|
|
uint32_t last_servo_out_us;
|
|
|
|
bool corked;
|
|
bool do_shutdown;
|
|
bool done_shutdown;
|
|
|
|
bool crc_is_ok;
|
|
bool detected_io_reset;
|
|
bool initialised;
|
|
bool is_chibios_backend;
|
|
|
|
uint32_t protocol_fail_count;
|
|
uint32_t protocol_count;
|
|
uint32_t total_errors;
|
|
uint32_t num_delayed;
|
|
uint32_t last_iocmu_timestamp_ms;
|
|
uint32_t read_status_errors;
|
|
uint32_t read_status_ok;
|
|
uint32_t last_rc_protocols;
|
|
|
|
// firmware upload
|
|
const char *fw_name = "io_firmware.bin";
|
|
const char *dshot_fw_name = "io_firmware_dshot.bin";
|
|
const uint8_t *fw;
|
|
uint32_t fw_size;
|
|
|
|
size_t write_wait(const uint8_t *pkt, uint8_t len);
|
|
bool upload_fw(void);
|
|
bool recv_byte_with_timeout(uint8_t *c, uint32_t timeout_ms);
|
|
bool recv_bytes(uint8_t *p, uint32_t count);
|
|
void drain(void);
|
|
bool send(uint8_t c);
|
|
bool send(const uint8_t *p, uint32_t count);
|
|
bool get_sync(uint32_t timeout = 40);
|
|
bool sync();
|
|
bool get_info(uint8_t param, uint32_t &val);
|
|
bool erase();
|
|
bool program(uint32_t fw_size);
|
|
bool verify_rev2(uint32_t fw_size);
|
|
bool verify_rev3(uint32_t fw_size_local);
|
|
bool reboot();
|
|
|
|
bool check_crc(void);
|
|
void handle_repeated_failures();
|
|
void check_iomcu_reset();
|
|
|
|
void write_log(); // handle onboard logging
|
|
|
|
static AP_IOMCU *singleton;
|
|
|
|
enum {
|
|
PROTO_NOP = 0x00,
|
|
PROTO_OK = 0x10,
|
|
PROTO_FAILED = 0x11,
|
|
PROTO_INSYNC = 0x12,
|
|
PROTO_INVALID = 0x13,
|
|
PROTO_BAD_SILICON_REV = 0x14,
|
|
PROTO_EOC = 0x20,
|
|
PROTO_GET_SYNC = 0x21,
|
|
PROTO_GET_DEVICE = 0x22,
|
|
PROTO_CHIP_ERASE = 0x23,
|
|
PROTO_CHIP_VERIFY = 0x24,
|
|
PROTO_PROG_MULTI = 0x27,
|
|
PROTO_READ_MULTI = 0x28,
|
|
PROTO_GET_CRC = 0x29,
|
|
PROTO_GET_OTP = 0x2a,
|
|
PROTO_GET_SN = 0x2b,
|
|
PROTO_GET_CHIP = 0x2c,
|
|
PROTO_SET_DELAY = 0x2d,
|
|
PROTO_GET_CHIP_DES = 0x2e,
|
|
PROTO_REBOOT = 0x30,
|
|
|
|
INFO_BL_REV = 1, /**< bootloader protocol revision */
|
|
BL_REV = 5, /**< supported bootloader protocol */
|
|
INFO_BOARD_ID = 2, /**< board type */
|
|
INFO_BOARD_REV = 3, /**< board revision */
|
|
INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */
|
|
|
|
PROG_MULTI_MAX = 248, /**< protocol max is 255, must be multiple of 4 */
|
|
};
|
|
};
|
|
|
|
namespace AP {
|
|
AP_IOMCU *iomcu(void);
|
|
};
|
|
|
|
#endif // HAL_WITH_IO_MCU
|