2019-03-22 23:32:56 -03:00
|
|
|
#pragma once
|
|
|
|
|
|
|
|
#include <stdint.h>
|
2018-05-02 08:22:17 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2019-03-22 23:32:56 -03:00
|
|
|
#include <AP_Common/AP_Common.h>
|
2018-10-31 21:59:54 -03:00
|
|
|
#include <AP_RCProtocol/AP_RCProtocol.h>
|
2018-05-02 08:22:17 -03:00
|
|
|
|
|
|
|
|
|
|
|
#include "ch.h"
|
2018-05-10 21:37:01 -03:00
|
|
|
#include "ioprotocol.h"
|
2018-05-02 08:22:17 -03:00
|
|
|
|
|
|
|
#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
|
|
|
|
#define SERVO_COUNT 8
|
2018-05-10 21:37:01 -03:00
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
class AP_IOMCU_FW {
|
|
|
|
public:
|
|
|
|
void process_io_packet();
|
|
|
|
|
2018-11-26 06:46:22 -04:00
|
|
|
struct IOPacket rx_io_packet, tx_io_packet;
|
2018-05-02 08:22:17 -03:00
|
|
|
|
2018-05-10 04:12:40 -03:00
|
|
|
void init();
|
2018-05-05 15:16:52 -03:00
|
|
|
void update();
|
|
|
|
void calculate_fw_crc(void);
|
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
void pwm_out_update();
|
|
|
|
void heater_update();
|
|
|
|
void rcin_update();
|
2018-05-05 15:16:52 -03:00
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
bool handle_code_write();
|
|
|
|
bool handle_code_read();
|
2018-05-05 15:16:52 -03:00
|
|
|
void schedule_reboot(uint32_t time_ms);
|
2018-05-10 22:43:28 -03:00
|
|
|
void safety_update();
|
2018-05-11 03:31:04 -03:00
|
|
|
void rcout_mode_update();
|
2018-10-31 21:59:54 -03:00
|
|
|
void rcin_serial_init();
|
|
|
|
void rcin_serial_update();
|
2018-10-28 20:23:31 -03:00
|
|
|
void page_status_update(void);
|
2018-10-30 23:10:51 -03:00
|
|
|
void fill_failsafe_pwm(void);
|
|
|
|
void run_mixer(void);
|
|
|
|
int16_t mix_input_angle(uint8_t channel, uint16_t radio_in) const;
|
|
|
|
int16_t mix_input_range(uint8_t channel, uint16_t radio_in) const;
|
|
|
|
uint16_t mix_output_angle(uint8_t channel, int16_t angle) const;
|
|
|
|
uint16_t mix_output_range(uint8_t channel, int16_t value) const;
|
2018-10-31 00:16:17 -03:00
|
|
|
int16_t mix_elevon_vtail(int16_t angle1, int16_t angle2, bool first_output) const;
|
2018-10-31 19:24:18 -03:00
|
|
|
void dsm_bind_step(void);
|
2018-05-10 21:37:01 -03:00
|
|
|
|
2019-08-13 21:07:48 -03:00
|
|
|
struct {
|
2018-05-02 08:22:17 -03:00
|
|
|
/* default to RSSI ADC functionality */
|
2018-05-10 21:37:01 -03:00
|
|
|
uint16_t features;
|
|
|
|
uint16_t arming;
|
|
|
|
uint16_t pwm_rates;
|
2018-05-02 08:22:17 -03:00
|
|
|
uint16_t pwm_defaultrate = 50;
|
|
|
|
uint16_t pwm_altrate = 200;
|
2018-05-10 21:37:01 -03:00
|
|
|
uint16_t relays_pad;
|
2018-05-02 08:22:17 -03:00
|
|
|
uint16_t vbatt_scale = 10000;
|
2018-05-05 15:16:52 -03:00
|
|
|
uint16_t reserved1;
|
|
|
|
uint16_t reserved2;
|
2018-05-10 21:37:01 -03:00
|
|
|
uint16_t set_debug;
|
|
|
|
uint16_t reboot_bl;
|
|
|
|
uint16_t crc[2];
|
2018-05-02 08:22:17 -03:00
|
|
|
uint16_t rc_thr_failsafe_us;
|
2018-05-05 15:16:52 -03:00
|
|
|
uint16_t reserved3;
|
2018-05-10 21:37:01 -03:00
|
|
|
uint16_t pwm_reverse;
|
|
|
|
uint16_t trim_roll;
|
|
|
|
uint16_t trim_pitch;
|
|
|
|
uint16_t trim_yaw;
|
2018-05-05 15:16:52 -03:00
|
|
|
uint16_t sbus_rate = 72;
|
2018-05-10 21:37:01 -03:00
|
|
|
uint16_t ignore_safety;
|
2018-05-02 08:22:17 -03:00
|
|
|
uint16_t heater_duty_cycle = 0xFFFFU;
|
|
|
|
uint16_t pwm_altclock = 1;
|
|
|
|
} reg_setup;
|
|
|
|
|
2018-10-30 21:07:47 -03:00
|
|
|
// CONFIG values
|
|
|
|
struct page_config config;
|
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
// PAGE_STATUS values
|
2018-05-10 21:37:01 -03:00
|
|
|
struct page_reg_status reg_status;
|
2018-05-02 08:22:17 -03:00
|
|
|
|
|
|
|
// PAGE_RAW_RCIN values
|
2018-05-10 21:37:01 -03:00
|
|
|
struct page_rc_input rc_input;
|
2019-08-13 21:07:48 -03:00
|
|
|
uint32_t rc_last_input_ms;
|
2018-05-02 08:22:17 -03:00
|
|
|
|
|
|
|
// PAGE_SERVO values
|
|
|
|
struct {
|
2018-05-10 21:37:01 -03:00
|
|
|
uint16_t pwm[IOMCU_MAX_CHANNELS];
|
2018-05-02 08:22:17 -03:00
|
|
|
} reg_servo;
|
|
|
|
|
2018-10-30 23:10:51 -03:00
|
|
|
// PAGE_DIRECT_PWM values
|
2018-05-02 08:22:17 -03:00
|
|
|
struct {
|
2018-05-10 21:37:01 -03:00
|
|
|
uint16_t pwm[IOMCU_MAX_CHANNELS];
|
2018-05-02 08:22:17 -03:00
|
|
|
} reg_direct_pwm;
|
|
|
|
|
2018-10-30 23:10:51 -03:00
|
|
|
// PAGE_FAILSAFE_PWM
|
|
|
|
struct {
|
|
|
|
uint16_t pwm[IOMCU_MAX_CHANNELS];
|
|
|
|
} reg_failsafe_pwm;
|
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
// output rates
|
|
|
|
struct {
|
|
|
|
uint16_t freq;
|
|
|
|
uint16_t chmask;
|
|
|
|
uint16_t default_freq = 50;
|
|
|
|
uint16_t sbus_rate_hz;
|
|
|
|
} rate;
|
|
|
|
|
2020-07-11 09:22:05 -03:00
|
|
|
// output mode values
|
|
|
|
struct {
|
|
|
|
uint16_t mask;
|
|
|
|
uint16_t mode;
|
|
|
|
} mode_out;
|
|
|
|
|
2018-10-30 21:07:47 -03:00
|
|
|
// MIXER values
|
|
|
|
struct page_mixing mixing;
|
2018-10-30 23:10:51 -03:00
|
|
|
|
2021-09-20 10:43:32 -03:00
|
|
|
// GPIO masks
|
|
|
|
struct page_GPIO GPIO;
|
|
|
|
uint8_t last_GPIO_channel_mask;
|
|
|
|
void GPIO_write();
|
|
|
|
|
2018-10-30 23:10:51 -03:00
|
|
|
// true when override channel active
|
|
|
|
bool override_active;
|
2019-10-20 10:47:14 -03:00
|
|
|
|
2018-10-29 02:51:43 -03:00
|
|
|
// sbus rate handling
|
|
|
|
uint32_t sbus_last_ms;
|
|
|
|
uint32_t sbus_interval_ms;
|
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
uint32_t fmu_data_received_time;
|
|
|
|
uint32_t last_heater_ms;
|
2018-05-05 15:16:52 -03:00
|
|
|
uint32_t reboot_time;
|
|
|
|
bool do_reboot;
|
|
|
|
bool update_default_rate;
|
|
|
|
bool update_rcout_freq;
|
2018-05-10 04:12:40 -03:00
|
|
|
bool has_heater;
|
2019-07-21 23:49:50 -03:00
|
|
|
const bool heater_pwm_polarity = IOMCU_IMU_HEATER_POLARITY;
|
2018-05-10 04:12:40 -03:00
|
|
|
uint32_t last_blue_led_ms;
|
2018-05-10 22:43:28 -03:00
|
|
|
uint32_t safety_update_ms;
|
|
|
|
uint32_t safety_button_counter;
|
|
|
|
uint8_t led_counter;
|
2018-05-11 03:31:04 -03:00
|
|
|
uint32_t last_loop_ms;
|
2020-07-11 09:22:05 -03:00
|
|
|
bool dshot_enabled;
|
2018-05-11 03:31:04 -03:00
|
|
|
bool oneshot_enabled;
|
2018-07-13 01:44:16 -03:00
|
|
|
bool brushed_enabled;
|
2018-05-11 03:31:04 -03:00
|
|
|
thread_t *thread_ctx;
|
2018-05-11 05:11:24 -03:00
|
|
|
bool last_safety_off;
|
2018-10-31 19:06:08 -03:00
|
|
|
uint32_t last_status_ms;
|
|
|
|
uint32_t last_ms;
|
|
|
|
uint32_t loop_counter;
|
2018-10-31 19:24:18 -03:00
|
|
|
uint8_t dsm_bind_state;
|
|
|
|
uint32_t last_dsm_bind_ms;
|
2018-10-31 19:31:45 -03:00
|
|
|
uint32_t last_failsafe_ms;
|
2018-05-10 04:12:40 -03:00
|
|
|
};
|
2018-05-02 08:22:17 -03:00
|
|
|
|
2018-11-01 04:24:41 -03:00
|
|
|
// GPIO macros
|
2019-05-06 07:39:47 -03:00
|
|
|
#define HEATER_SET(on) palWriteLine(HAL_GPIO_PIN_HEATER, (on));
|
2018-11-01 04:24:41 -03:00
|
|
|
#define BLUE_TOGGLE() palToggleLine(HAL_GPIO_PIN_HEATER);
|
|
|
|
#define AMBER_SET(on) palWriteLine(HAL_GPIO_PIN_AMBER_LED, !(on));
|
|
|
|
#define SPEKTRUM_POWER(on) palWriteLine(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, on);
|
|
|
|
#define SPEKTRUM_SET(on) palWriteLine(HAL_GPIO_PIN_SPEKTRUM_OUT, on);
|