zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

IOMCU: add application for ioboard firmware

master
Siddharth Purohit 7 years ago committed by Andrew Tridgell
parent
17b61f72a3
commit
4608e90913
3 changed files with 536 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 382
      libraries/AP_IOMCU/iofirmware/iofirmware.cpp
  2. 130
      libraries/AP_IOMCU/iofirmware/iofirmware.h
  3. 24
      libraries/AP_IOMCU/iofirmware/wscript

382
libraries/AP_IOMCU/iofirmware/iofirmware.cpp
Unescape View File

@ -0,0 +1,382 @@ @@ -0,0 +1,382 @@
//
// Simple test for the AP_AHRS interface
//
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_Math/crc.h>
#include "iofirmware.h"
#include "hal.h"
extern const AP_HAL::HAL &hal;
#pragma GCC optimize("O0")
#define PKT_MAX_REGS 32
AP_IOMCU_FW iomcu;
/*
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,
};
// 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_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
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;
void dma_rx_end_cb(UARTDriver *uart)
{
osalSysLockFromISR();
uart->usart->CR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
(void)uart->usart->SR;
(void)uart->usart->DR;
(void)uart->usart->DR;
dmaStreamDisable(uart->dmarx);
dmaStreamDisable(uart->dmatx);
iomcu.process_io_packet();
num_total_rx++;
num_dma_complete_rx = num_total_rx - num_idle_rx;
dmaStreamSetMemory0(uart->dmarx, &iomcu.rx_io_packet);
dmaStreamSetTransactionSize(uart->dmarx, sizeof(iomcu.rx_io_packet));
dmaStreamSetMode(uart->dmarx, uart->dmamode | STM32_DMA_CR_DIR_P2M |
STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE);
dmaStreamEnable(uart->dmarx);
uart->usart->CR3 |= USART_CR3_DMAR;
dmaStreamSetMemory0(uart->dmatx, &iomcu.tx_io_packet);
dmaStreamSetTransactionSize(uart->dmatx, iomcu.tx_io_packet.get_size());
dmaStreamSetMode(uart->dmatx, uart->dmamode | STM32_DMA_CR_DIR_M2P |
STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE);
dmaStreamEnable(uart->dmatx);
uart->usart->CR3 |= USART_CR3_DMAT;
osalSysUnlockFromISR();
}
void idle_rx_handler(UARTDriver *uart) {
volatile uint16_t sr = uart->usart->SR;
if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */
USART_SR_NE | /* noise error - we have lost a byte due to noise */
USART_SR_FE)) { /* framing error - start/stop bit lost or line break */
/* send a line break - this will abort transmission/reception on the other end */
osalSysLockFromISR();
uart->usart->CR1 |= USART_CR1_SBK;
num_rx_error++;
uart->usart->CR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
(void)uart->usart->SR;
(void)uart->usart->DR;
(void)uart->usart->DR;
dmaStreamDisable(uart->dmarx);
dmaStreamDisable(uart->dmatx);
dmaStreamSetMemory0(uart->dmarx, &iomcu.rx_io_packet);
dmaStreamSetTransactionSize(uart->dmarx, sizeof(iomcu.rx_io_packet));
dmaStreamSetMode(uart->dmarx, uart->dmamode | STM32_DMA_CR_DIR_P2M |
STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE);
dmaStreamEnable(uart->dmarx);
uart->usart->CR3 |= USART_CR3_DMAR;
osalSysUnlockFromISR();
return;
}
if(sr & USART_SR_IDLE) {
dma_rx_end_cb(uart);
num_idle_rx++;
}
}
/*
* UART driver configuration structure.
*/
static UARTConfig uart_cfg = {
NULL,
NULL,
dma_rx_end_cb,
NULL,
NULL,
idle_rx_handler,
1500000, //1.5MBit
USART_CR1_IDLEIE,
0,
0
};
void setup(void)
{
hal.gpio->init();
hal.rcin->init();
hal.rcout->init();
for (uint8_t i = 0; i< 14; i++) {
hal.rcout->enable_ch(i);
}
(void)uart_cfg;
uartStart(&UARTD2, &uart_cfg);
uartStartReceive(&UARTD2, sizeof(iomcu.rx_io_packet), &iomcu.rx_io_packet);
}
void loop(void)
{
iomcu.pwm_out_update();
iomcu.heater_update();
iomcu.rcin_update();
}
void AP_IOMCU_FW::pwm_out_update()
{
//TODO: PWM mixing
memcpy(reg_servo.pwm, reg_direct_pwm.pwm, sizeof(reg_direct_pwm));
for(uint8_t i = 0; i < SERVO_COUNT; i++) {
if (reg_servo.pwm[i] != 0) {
hal.rcout->write(i, reg_servo.pwm[i]);
}
}
}
void AP_IOMCU_FW::heater_update()
{
if (reg_setup.heater_duty_cycle == 0 || (AP_HAL::millis() - last_heater_ms > 3000UL)) {
hal.gpio->write(0, 0);
} else {
uint8_t cycle = ((AP_HAL::millis() / 10UL) % 100U);
hal.gpio->write(0, !(cycle >= reg_setup.heater_duty_cycle));
}
}
void AP_IOMCU_FW::rcin_update()
{
if (hal.rcin->new_input()) {
rc_input.count = hal.rcin->num_channels();
rc_input.flags_rc_ok = true;
for (uint8_t i = 0; i < IOMCU_MAX_CHANNELS; i++) {
rc_input.pwm[i] = hal.rcin->read(i);
}
rc_input.last_input_us = AP_HAL::micros();
}
}
void AP_IOMCU_FW::process_io_packet()
{
uint8_t rx_crc = rx_io_packet.crc;
rx_io_packet.crc = 0;
uint8_t calc_crc = crc_crc8((const uint8_t *)&rx_io_packet, rx_io_packet.get_size());
if (rx_crc != calc_crc) {
memset(&tx_io_packet, 0xFF, sizeof(tx_io_packet));
tx_io_packet.count = 0;
tx_io_packet.code = CODE_CORRUPT;
tx_io_packet.crc = 0;
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
num_bad_crc++;
return;
}
switch(rx_io_packet.code) {
case CODE_READ:
{
num_code_read++;
handle_code_read();
}
break;
case CODE_WRITE:
{
num_write_pkt++;
handle_code_write();
}
break;
default:
{
num_unknown_pkt++;
}
break;
}
}
bool AP_IOMCU_FW::handle_code_read()
{
uint16_t *values = NULL;
#define COPY_PAGE(_page_name) \
do { \
values = (uint16_t *)&_page_name; \
tx_io_packet.count = sizeof(_page_name) / sizeof(uint16_t); \
} while(0);
switch(rx_io_packet.page) {
case PAGE_SETUP:
COPY_PAGE(reg_setup);
break;
case PAGE_RAW_RCIN:
COPY_PAGE(rc_input);
break;
case PAGE_STATUS:
COPY_PAGE(reg_status);
break;
case PAGE_SERVOS:
COPY_PAGE(reg_servo);
break;
default:
return false;
}
last_page = rx_io_packet.page;
last_offset = rx_io_packet.offset;
/* if the offset is at or beyond the end of the page, we have no data */
if (rx_io_packet.offset >= tx_io_packet.count) {
return false;
}
/* correct the data pointer and count for the offset */
values += rx_io_packet.offset;
tx_io_packet.count -= rx_io_packet.offset;
memcpy(tx_io_packet.regs, values, sizeof(uint16_t)*tx_io_packet.count);
tx_io_packet.crc = 0;
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
return true;
}
bool AP_IOMCU_FW::handle_code_write()
{
switch(rx_io_packet.page) {
case PAGE_SETUP:
switch(rx_io_packet.offset) {
case PAGE_REG_SETUP_ARMING:
reg_setup.arming = rx_io_packet.regs[0];
break;
case PAGE_REG_SETUP_FORCE_SAFETY_OFF:
if (rx_io_packet.regs[0] == FORCE_SAFETY_MAGIC) {
reg_status.flag_safety_off = true;
} else {
return false;
}
break;
case PAGE_REG_SETUP_FORCE_SAFETY_ON:
if (rx_io_packet.regs[0] == FORCE_SAFETY_MAGIC) {
reg_status.flag_safety_off = false;
} else {
return false;
}
break;
case PAGE_REG_SETUP_ALTRATE:
if (rx_io_packet.regs[0] < 25 && reg_setup.pwm_altclock == 1) {
rx_io_packet.regs[0] = 25;
}
if (rx_io_packet.regs[0] > 400 && reg_setup.pwm_altclock == 1) {
rx_io_packet.regs[0] = 400;
}
reg_setup.pwm_altrate = rx_io_packet.regs[0];
hal.rcout->set_freq(reg_setup.pwm_rates, reg_setup.pwm_altrate);
break;
case PAGE_REG_SETUP_PWM_RATE_MASK:
reg_setup.pwm_rates = rx_io_packet.regs[0];
hal.rcout->set_freq(reg_setup.pwm_rates, reg_setup.pwm_altrate);
break;
case PAGE_REG_SETUP_DEFAULTRATE:
if (rx_io_packet.regs[0] < 25 && reg_setup.pwm_altclock == 1) {
rx_io_packet.regs[0] = 25;
}
if (rx_io_packet.regs[0] > 400 && reg_setup.pwm_altclock == 1) {
rx_io_packet.regs[0] = 400;
}
reg_setup.pwm_defaultrate = rx_io_packet.regs[0];
hal.rcout->set_default_rate(reg_setup.pwm_defaultrate);
break;
case PAGE_REG_SETUP_SBUS_RATE:
break;
case PAGE_REG_SETUP_FEATURES:
reg_setup.features = rx_io_packet.regs[0];
/* disable the conflicting options with SBUS 1 */
if (reg_setup.features & (P_SETUP_FEATURES_SBUS1_OUT)) {
reg_setup.features &= ~(P_SETUP_FEATURES_PWM_RSSI |
P_SETUP_FEATURES_ADC_RSSI |
P_SETUP_FEATURES_SBUS2_OUT);
}
break;
case PAGE_REG_SETUP_HEATER_DUTY_CYCLE:
reg_setup.heater_duty_cycle = rx_io_packet.regs[0];
last_heater_ms = AP_HAL::millis();
break;
default:
break;
}
break;
case PAGE_DIRECT_PWM:
{
/* copy channel data */
uint8_t i = 0, offset = rx_io_packet.offset, num_values = rx_io_packet.count;
while ((offset < IOMCU_MAX_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
if (rx_io_packet.regs[i] != PWM_IGNORE_THIS_CHANNEL) {
reg_direct_pwm.pwm[offset] = rx_io_packet.regs[i];
}
offset++;
num_values--;
i++;
}
fmu_data_received_time = AP_HAL::millis();
reg_status.flag_fmu_ok = true;
reg_status.flag_raw_pwm = true;
//TODO: Oneshot support
break;
}
default:
break;
}
memset(&tx_io_packet, 0xFF, sizeof(tx_io_packet));
tx_io_packet.count = 0;
tx_io_packet.code = CODE_SUCCESS;
tx_io_packet.crc = 0;
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
return true;
}
AP_HAL_MAIN();

130
libraries/AP_IOMCU/iofirmware/iofirmware.h
Unescape View File

@ -0,0 +1,130 @@ @@ -0,0 +1,130 @@
#include <AP_HAL/AP_HAL.h>
#include "ch.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};
void pwm_out_update();
void heater_update();
void rcin_update();
private:
bool handle_code_write();
bool handle_code_read();
struct PACKED {
/* default to RSSI ADC functionality */
uint16_t features = 0;
uint16_t arming = 0;
uint16_t pwm_rates = 0;
uint16_t pwm_defaultrate = 50;
uint16_t pwm_altrate = 200;
uint16_t sbus_rate = 72;
uint16_t relays_pad = 0;
uint16_t vbatt_scale = 10000;
uint16_t set_debug = 0;
uint16_t reboot_bl = 0;
uint16_t crc[2] = {0};
uint16_t rc_thr_failsafe_us;
uint16_t pwm_reverse = 0;
uint16_t trim_roll = 0;
uint16_t trim_pitch = 0;
uint16_t trim_yaw = 0;
uint16_t ignore_safety = 0;
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};
// 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};
// PAGE_SERVO values
struct {
uint16_t pwm[IOMCU_MAX_CHANNELS] = {0};
} reg_servo;
// PAGE_SERVO values
struct {
uint16_t pwm[IOMCU_MAX_CHANNELS]= {0};
} reg_direct_pwm;
// output rates
struct {
uint16_t freq;
uint16_t chmask;
uint16_t default_freq = 50;
uint16_t sbus_rate_hz;
} rate;
uint8_t last_page;
uint8_t last_offset;
uint32_t fmu_data_received_time;
uint32_t last_heater_ms;
};

24
libraries/AP_IOMCU/iofirmware/wscript
Unescape View File

@ -0,0 +1,24 @@ @@ -0,0 +1,24 @@
#!/usr/bin/env python
# encoding: utf-8
def build(bld):
bld.ap_stlib(
name= 'iofirmware_libs',
ap_vehicle='iofirmware',
ap_libraries= [
'AP_Buffer',
'AP_Common',
'AP_HAL',
'AP_HAL_Empty',
'AP_Math',
'AP_RCProtocol',
],
exclude_src=[
'libraries/AP_HAL_ChibiOS/Storage.cpp'
]
)
bld.ap_program(
program_name='iofirmware',
use='iofirmware_libs',
program_groups=['bin'],
)
Write Preview
Loading…
Cancel
Save