2018-10-28 20:23:31 -03:00
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
/*
|
|
|
|
IOMCU main firmware
|
|
|
|
*/
|
2018-05-02 08:22:17 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
|
|
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
#include <AP_Math/crc.h>
|
|
|
|
#include "iofirmware.h"
|
2018-10-05 21:33:30 -03:00
|
|
|
#include <AP_HAL_ChibiOS/RCInput.h>
|
2018-10-31 01:50:31 -03:00
|
|
|
#include <AP_HAL_ChibiOS/RCOutput.h>
|
2018-10-28 20:23:31 -03:00
|
|
|
#include "analog.h"
|
2018-10-31 19:13:28 -03:00
|
|
|
#include "rc.h"
|
2019-04-23 22:33:42 -03:00
|
|
|
#include <AP_HAL_ChibiOS/hwdef/common/watchdog.h>
|
2018-10-29 02:51:43 -03:00
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
extern const AP_HAL::HAL &hal;
|
2018-05-02 09:57:05 -03:00
|
|
|
|
2018-11-26 06:46:22 -04:00
|
|
|
// we build this file with optimisation to lower the interrupt
|
|
|
|
// latency. This helps reduce the chance of losing an RC input byte
|
|
|
|
// due to missing a UART interrupt
|
2019-09-27 17:45:12 -03:00
|
|
|
#pragma GCC optimize("O2")
|
2018-05-11 03:31:04 -03:00
|
|
|
|
2018-05-10 21:37:01 -03:00
|
|
|
static AP_IOMCU_FW iomcu;
|
2018-05-02 08:22:17 -03:00
|
|
|
|
|
|
|
void setup();
|
|
|
|
void loop();
|
|
|
|
|
2023-06-17 19:26:56 -03:00
|
|
|
#undef CH_DBG_ENABLE_STACK_CHECK
|
|
|
|
#define CH_DBG_ENABLE_STACK_CHECK FALSE
|
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
|
|
|
|
2023-02-10 21:39:14 -04:00
|
|
|
/*
|
|
|
|
enable testing of IOMCU reset using safety switch
|
|
|
|
a value of 0 means normal operation
|
|
|
|
a value of 1 means test with watchdog
|
|
|
|
a value of 2 means test with reboot
|
|
|
|
*/
|
|
|
|
#define IOMCU_ENABLE_RESET_TEST 0
|
2023-08-14 04:43:33 -03:00
|
|
|
|
2023-12-21 11:06:36 -04:00
|
|
|
//#define IOMCU_LOOP_TIMING_DEBUG
|
2023-06-01 12:06:52 -03:00
|
|
|
// enable timing GPIO pings
|
|
|
|
#ifdef IOMCU_LOOP_TIMING_DEBUG
|
|
|
|
#undef TOGGLE_PIN_DEBUG
|
|
|
|
#define TOGGLE_PIN_DEBUG(pin) do { palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0)
|
|
|
|
#endif
|
2019-04-23 22:33:42 -03:00
|
|
|
|
2018-05-11 03:31:04 -03:00
|
|
|
// pending events on the main thread
|
|
|
|
enum ioevents {
|
2023-06-01 12:06:52 -03:00
|
|
|
IOEVENT_PWM = EVENT_MASK(1),
|
|
|
|
IOEVENT_TX_BEGIN = EVENT_MASK(2),
|
|
|
|
IOEVENT_TX_END = EVENT_MASK(3),
|
2018-05-11 03:31:04 -03:00
|
|
|
};
|
|
|
|
|
2023-06-01 12:06:52 -03:00
|
|
|
// see https://github.com/MaJerle/stm32-usart-uart-dma-rx-tx for a discussion of how to run
|
|
|
|
// separate tx and rx streams
|
|
|
|
static void setup_rx_dma(hal_uart_driver* uart)
|
2018-05-02 08:22:17 -03:00
|
|
|
{
|
2023-06-01 12:06:52 -03:00
|
|
|
uart->usart->CR3 &= ~USART_CR3_DMAR;
|
2018-05-02 08:22:17 -03:00
|
|
|
dmaStreamDisable(uart->dmarx);
|
|
|
|
dmaStreamSetMemory0(uart->dmarx, &iomcu.rx_io_packet);
|
|
|
|
dmaStreamSetTransactionSize(uart->dmarx, sizeof(iomcu.rx_io_packet));
|
2023-06-01 12:06:52 -03:00
|
|
|
dmaStreamSetPeripheral(uart->dmarx, &(uart->usart->DR));
|
2021-02-19 16:48:33 -04:00
|
|
|
dmaStreamSetMode(uart->dmarx, uart->dmarxmode | STM32_DMA_CR_DIR_P2M |
|
2018-09-14 07:06:59 -03:00
|
|
|
STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE);
|
2018-05-02 08:22:17 -03:00
|
|
|
dmaStreamEnable(uart->dmarx);
|
|
|
|
uart->usart->CR3 |= USART_CR3_DMAR;
|
2023-06-01 12:06:52 -03:00
|
|
|
}
|
2018-05-02 08:22:17 -03:00
|
|
|
|
2023-06-01 12:06:52 -03:00
|
|
|
static void setup_tx_dma(hal_uart_driver* uart)
|
|
|
|
{
|
|
|
|
uart->usart->CR3 &= ~USART_CR3_DMAT;
|
|
|
|
dmaStreamDisable(uart->dmatx);
|
2018-05-02 08:22:17 -03:00
|
|
|
dmaStreamSetMemory0(uart->dmatx, &iomcu.tx_io_packet);
|
|
|
|
dmaStreamSetTransactionSize(uart->dmatx, iomcu.tx_io_packet.get_size());
|
2023-06-01 12:06:52 -03:00
|
|
|
// starting the UART allocates the peripheral statically, so we need to reinstate it after swapping
|
|
|
|
dmaStreamSetPeripheral(uart->dmatx, &(uart->usart->DR));
|
2021-02-19 16:48:33 -04:00
|
|
|
dmaStreamSetMode(uart->dmatx, uart->dmatxmode | STM32_DMA_CR_DIR_M2P |
|
2018-09-14 07:06:59 -03:00
|
|
|
STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE);
|
2023-06-01 12:06:52 -03:00
|
|
|
// enable transmission complete interrupt
|
2024-05-30 12:31:36 -03:00
|
|
|
uart->usart->SR &= ~USART_SR_TC;
|
2023-06-01 12:06:52 -03:00
|
|
|
uart->usart->CR1 |= USART_CR1_TCIE;
|
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
dmaStreamEnable(uart->dmatx);
|
2023-06-01 12:06:52 -03:00
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
uart->usart->CR3 |= USART_CR3_DMAT;
|
2023-06-01 12:06:52 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
static void dma_rx_end_cb(hal_uart_driver *uart)
|
|
|
|
{
|
|
|
|
chSysLockFromISR();
|
|
|
|
uart->usart->CR3 &= ~USART_CR3_DMAR;
|
|
|
|
|
|
|
|
dmaStreamDisable(uart->dmarx);
|
|
|
|
|
|
|
|
iomcu.process_io_packet();
|
|
|
|
|
|
|
|
setup_rx_dma(uart);
|
|
|
|
|
|
|
|
#if AP_HAL_SHARED_DMA_ENABLED
|
2023-06-09 17:42:29 -03:00
|
|
|
// indicate that a response needs to be sent
|
2023-06-01 12:06:52 -03:00
|
|
|
uint32_t mask = chEvtGetAndClearEventsI(IOEVENT_TX_BEGIN);
|
|
|
|
if (mask) {
|
|
|
|
iomcu.reg_status.err_lock++;
|
|
|
|
}
|
|
|
|
// the FMU code waits 10ms for a reply so this should be easily fast enough
|
|
|
|
chEvtSignalI(iomcu.thread_ctx, IOEVENT_TX_BEGIN);
|
|
|
|
#else
|
|
|
|
setup_tx_dma(uart);
|
|
|
|
#endif
|
2023-05-24 17:43:17 -03:00
|
|
|
chSysUnlockFromISR();
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
|
|
|
|
2023-06-01 12:06:52 -03:00
|
|
|
static void dma_tx_end_cb(hal_uart_driver *uart)
|
2018-05-10 21:37:01 -03:00
|
|
|
{
|
2023-06-01 12:06:52 -03:00
|
|
|
// DMA stream has already been disabled at this point
|
|
|
|
uart->usart->CR3 &= ~USART_CR3_DMAT;
|
|
|
|
|
|
|
|
(void)uart->usart->SR;
|
|
|
|
(void)uart->usart->DR;
|
|
|
|
(void)uart->usart->DR;
|
|
|
|
|
2023-06-23 17:15:14 -03:00
|
|
|
#ifdef HAL_GPIO_LINE_GPIO108
|
2023-06-01 12:06:52 -03:00
|
|
|
TOGGLE_PIN_DEBUG(108);
|
|
|
|
TOGGLE_PIN_DEBUG(108);
|
2023-06-23 17:15:14 -03:00
|
|
|
#endif
|
2023-12-21 11:06:36 -04:00
|
|
|
#if AP_HAL_SHARED_DMA_ENABLED
|
|
|
|
chSysLockFromISR();
|
2023-06-24 15:16:43 -03:00
|
|
|
chEvtSignalI(iomcu.thread_ctx, IOEVENT_TX_END);
|
2023-12-21 11:06:36 -04:00
|
|
|
chSysUnlockFromISR();
|
|
|
|
#endif
|
2023-06-01 12:06:52 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/* replacement for ChibiOS uart_lld_serve_interrupt() */
|
|
|
|
static void idle_rx_handler(hal_uart_driver *uart)
|
|
|
|
{
|
|
|
|
volatile uint16_t sr;
|
|
|
|
sr = uart->usart->SR; /* SR reset step 1.*/
|
|
|
|
uint32_t cr1 = uart->usart->CR1;
|
2018-05-05 15:16:52 -03:00
|
|
|
|
|
|
|
if (sr & (USART_SR_LBD | USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */
|
2018-09-14 07:06:59 -03:00
|
|
|
USART_SR_NE | /* noise error - we have lost a byte due to noise */
|
|
|
|
USART_SR_FE |
|
|
|
|
USART_SR_PE)) { /* framing error - start/stop bit lost or line break */
|
2023-12-21 11:06:36 -04:00
|
|
|
|
|
|
|
(void)uart->usart->DR; /* SR reset step 2 - clear ORE | FE.*/
|
|
|
|
|
2018-09-14 07:06:59 -03:00
|
|
|
/* send a line break - this will abort transmission/reception on the other end */
|
2023-05-24 17:43:17 -03:00
|
|
|
chSysLockFromISR();
|
2018-05-05 15:16:52 -03:00
|
|
|
uart->usart->SR = ~USART_SR_LBD;
|
2023-06-01 12:06:52 -03:00
|
|
|
uart->usart->CR1 = cr1 | USART_CR1_SBK;
|
2023-12-21 11:06:36 -04:00
|
|
|
|
2019-08-13 21:07:48 -03:00
|
|
|
iomcu.reg_status.num_errors++;
|
|
|
|
iomcu.reg_status.err_uart++;
|
2023-06-01 12:06:52 -03:00
|
|
|
|
2023-12-21 11:06:36 -04:00
|
|
|
/* disable RX DMA */
|
2023-06-01 12:06:52 -03:00
|
|
|
uart->usart->CR3 &= ~USART_CR3_DMAR;
|
|
|
|
|
|
|
|
setup_rx_dma(uart);
|
2018-05-02 08:22:17 -03:00
|
|
|
|
2023-05-24 17:43:17 -03:00
|
|
|
chSysUnlockFromISR();
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
|
|
|
|
2023-06-01 12:06:52 -03:00
|
|
|
if ((sr & USART_SR_TC) && (cr1 & USART_CR1_TCIE)) {
|
|
|
|
/* TC interrupt cleared and disabled.*/
|
|
|
|
uart->usart->SR &= ~USART_SR_TC;
|
|
|
|
uart->usart->CR1 = cr1 & ~USART_CR1_TCIE;
|
2023-12-21 11:06:36 -04:00
|
|
|
#ifdef HAL_GPIO_LINE_GPIO105
|
|
|
|
TOGGLE_PIN_DEBUG(105);
|
|
|
|
TOGGLE_PIN_DEBUG(105);
|
|
|
|
#endif
|
2023-06-01 12:06:52 -03:00
|
|
|
/* End of transmission, a callback is generated.*/
|
2023-12-21 11:06:36 -04:00
|
|
|
dma_tx_end_cb(uart);
|
2023-06-01 12:06:52 -03:00
|
|
|
}
|
|
|
|
|
2023-12-21 11:06:36 -04:00
|
|
|
if ((sr & USART_SR_IDLE) && (cr1 & USART_CR1_IDLEIE)) {
|
|
|
|
(void)uart->usart->DR; /* SR reset step 2 - clear IDLE.*/
|
|
|
|
|
2023-06-01 12:06:52 -03:00
|
|
|
/* the DMA size is the maximum packet size, but smaller packets are perfectly possible leading to
|
|
|
|
an IDLE ISR. The data still must be processed. */
|
2023-12-21 11:06:36 -04:00
|
|
|
|
|
|
|
/* End of receive, a callback is generated.*/
|
2018-05-02 08:22:17 -03:00
|
|
|
dma_rx_end_cb(uart);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2023-06-01 12:06:52 -03:00
|
|
|
using namespace ChibiOS;
|
|
|
|
|
|
|
|
#if AP_HAL_SHARED_DMA_ENABLED
|
|
|
|
/*
|
|
|
|
copy of uart_lld_serve_tx_end_irq() from ChibiOS hal_uart_lld
|
|
|
|
that is re-instated upon switching the DMA channel
|
|
|
|
*/
|
2023-06-17 19:26:56 -03:00
|
|
|
static void uart_lld_serve_tx_end_irq(hal_uart_driver *uart, uint32_t flags)
|
|
|
|
{
|
2023-06-01 12:06:52 -03:00
|
|
|
dmaStreamDisable(uart->dmatx);
|
|
|
|
|
|
|
|
/* A callback is generated, if enabled, after a completed transfer.*/
|
|
|
|
_uart_tx1_isr_code(uart);
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_IOMCU_FW::tx_dma_allocate(Shared_DMA *ctx)
|
|
|
|
{
|
|
|
|
hal_uart_driver *uart = &UARTD2;
|
|
|
|
chSysLock();
|
|
|
|
if (uart->dmatx == nullptr) {
|
|
|
|
uart->dmatx = dmaStreamAllocI(STM32_UART_USART2_TX_DMA_STREAM,
|
|
|
|
STM32_UART_USART2_IRQ_PRIORITY,
|
|
|
|
(stm32_dmaisr_t)uart_lld_serve_tx_end_irq,
|
|
|
|
(void *)uart);
|
|
|
|
}
|
|
|
|
chSysUnlock();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
deallocate DMA channel
|
|
|
|
*/
|
|
|
|
void AP_IOMCU_FW::tx_dma_deallocate(Shared_DMA *ctx)
|
|
|
|
{
|
|
|
|
hal_uart_driver *uart = &UARTD2;
|
|
|
|
chSysLock();
|
|
|
|
if (uart->dmatx != nullptr) {
|
|
|
|
// defensively make sure the DMA is fully shutdown before swapping
|
|
|
|
uart->usart->CR3 &= ~USART_CR3_DMAT;
|
|
|
|
dmaStreamDisable(uart->dmatx);
|
|
|
|
dmaStreamSetPeripheral(uart->dmatx, nullptr);
|
|
|
|
dmaStreamFreeI(uart->dmatx);
|
|
|
|
uart->dmatx = nullptr;
|
|
|
|
}
|
|
|
|
chSysUnlock();
|
|
|
|
}
|
|
|
|
#endif // AP_HAL_SHARED_DMA_ENABLED
|
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
/*
|
|
|
|
* UART driver configuration structure.
|
|
|
|
*/
|
|
|
|
static UARTConfig uart_cfg = {
|
2018-09-14 07:06:59 -03:00
|
|
|
nullptr,
|
2023-06-01 12:06:52 -03:00
|
|
|
dma_tx_end_cb,
|
2018-09-14 07:06:59 -03:00
|
|
|
dma_rx_end_cb,
|
|
|
|
nullptr,
|
2023-12-21 11:06:36 -04:00
|
|
|
nullptr, // error
|
|
|
|
idle_rx_handler, // global irq
|
|
|
|
nullptr, // idle
|
2018-09-14 07:06:59 -03:00
|
|
|
1500000, //1.5MBit
|
|
|
|
USART_CR1_IDLEIE,
|
|
|
|
0,
|
|
|
|
0
|
2018-05-02 08:22:17 -03:00
|
|
|
};
|
|
|
|
|
|
|
|
void setup(void)
|
|
|
|
{
|
|
|
|
hal.rcin->init();
|
|
|
|
hal.rcout->init();
|
2018-05-10 04:12:40 -03:00
|
|
|
iomcu.init();
|
2018-09-14 07:06:59 -03:00
|
|
|
|
2018-05-05 15:16:52 -03:00
|
|
|
iomcu.calculate_fw_crc();
|
2023-06-01 12:06:52 -03:00
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
uartStart(&UARTD2, &uart_cfg);
|
|
|
|
uartStartReceive(&UARTD2, sizeof(iomcu.rx_io_packet), &iomcu.rx_io_packet);
|
2023-06-09 17:42:29 -03:00
|
|
|
#if AP_HAL_SHARED_DMA_ENABLED
|
|
|
|
iomcu.tx_dma_handle->unlock();
|
|
|
|
#endif
|
2023-06-01 12:06:52 -03:00
|
|
|
// disable the pieces from the UART which will get enabled later
|
|
|
|
chSysLock();
|
|
|
|
UARTD2.usart->CR3 &= ~USART_CR3_DMAT;
|
|
|
|
chSysUnlock();
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void loop(void)
|
|
|
|
{
|
2018-05-05 15:16:52 -03:00
|
|
|
iomcu.update();
|
|
|
|
}
|
|
|
|
|
2018-05-10 04:12:40 -03:00
|
|
|
void AP_IOMCU_FW::init()
|
|
|
|
{
|
2018-11-01 03:39:24 -03:00
|
|
|
// the first protocol version must be 4 to allow downgrade to
|
|
|
|
// old NuttX based firmwares
|
2018-10-30 21:24:51 -03:00
|
|
|
config.protocol_version = IOMCU_PROTOCOL_VERSION;
|
2018-11-01 03:39:24 -03:00
|
|
|
config.protocol_version2 = IOMCU_PROTOCOL_VERSION2;
|
2023-06-17 19:26:56 -03:00
|
|
|
config.mcuid = (*(uint32_t *)DBGMCU_BASE);
|
2023-06-26 11:39:23 -03:00
|
|
|
#if defined(STM32F103xB) || defined(STM32F103x8)
|
|
|
|
if (config.mcuid == 0) {
|
|
|
|
// Errata 2.2.2 - Debug registers cannot be read by user software
|
|
|
|
config.mcuid = 0x20036410; // STM32F10x (Medium Density) rev Y
|
|
|
|
}
|
2023-06-17 19:26:56 -03:00
|
|
|
#endif
|
2023-06-24 15:16:43 -03:00
|
|
|
config.cpuid = SCB->CPUID;
|
2018-10-30 21:24:51 -03:00
|
|
|
|
2018-05-11 03:31:04 -03:00
|
|
|
thread_ctx = chThdGetSelfX();
|
|
|
|
|
2023-06-01 12:06:52 -03:00
|
|
|
#if AP_HAL_SHARED_DMA_ENABLED
|
2024-05-26 22:24:12 -03:00
|
|
|
tx_dma_handle = NEW_NOTHROW ChibiOS::Shared_DMA(STM32_UART_USART2_TX_DMA_STREAM, SHARED_DMA_NONE,
|
2023-06-01 12:06:52 -03:00
|
|
|
FUNCTOR_BIND_MEMBER(&AP_IOMCU_FW::tx_dma_allocate, void, Shared_DMA *),
|
|
|
|
FUNCTOR_BIND_MEMBER(&AP_IOMCU_FW::tx_dma_deallocate, void, Shared_DMA *));
|
|
|
|
tx_dma_handle->lock();
|
|
|
|
// deallocate so that the uart initializes correctly
|
|
|
|
tx_dma_deallocate(tx_dma_handle);
|
|
|
|
#endif
|
|
|
|
|
2018-05-10 04:12:40 -03:00
|
|
|
if (palReadLine(HAL_GPIO_PIN_IO_HW_DETECT1) == 1 && palReadLine(HAL_GPIO_PIN_IO_HW_DETECT2) == 0) {
|
|
|
|
has_heater = true;
|
|
|
|
}
|
2018-10-28 20:23:31 -03:00
|
|
|
|
2019-05-06 07:39:47 -03:00
|
|
|
//Set Heater pin mode
|
|
|
|
if (heater_pwm_polarity) {
|
|
|
|
palSetLineMode(HAL_GPIO_PIN_HEATER, PAL_MODE_OUTPUT_PUSHPULL);
|
|
|
|
} else {
|
|
|
|
palSetLineMode(HAL_GPIO_PIN_HEATER, PAL_MODE_OUTPUT_OPENDRAIN);
|
|
|
|
}
|
|
|
|
|
2018-10-28 20:23:31 -03:00
|
|
|
adc_init();
|
2018-10-31 21:59:54 -03:00
|
|
|
rcin_serial_init();
|
2018-10-31 19:06:08 -03:00
|
|
|
|
2018-10-31 19:24:18 -03:00
|
|
|
// power on spektrum port
|
|
|
|
palSetLineMode(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, PAL_MODE_OUTPUT_PUSHPULL);
|
2018-11-01 04:24:41 -03:00
|
|
|
SPEKTRUM_POWER(1);
|
2018-10-31 19:24:18 -03:00
|
|
|
|
2023-06-01 12:06:52 -03:00
|
|
|
// we generally do no allocations after setup completes
|
2018-10-31 19:06:08 -03:00
|
|
|
reg_status.freemem = hal.util->available_memory();
|
2019-04-21 23:46:58 -03:00
|
|
|
|
|
|
|
if (hal.util->was_watchdog_safety_off()) {
|
|
|
|
hal.rcout->force_safety_off();
|
|
|
|
reg_status.flag_safety_off = true;
|
|
|
|
}
|
2018-05-10 04:12:40 -03:00
|
|
|
}
|
2018-09-14 07:06:59 -03:00
|
|
|
|
2018-05-10 04:12:40 -03:00
|
|
|
|
2023-05-24 17:43:17 -03:00
|
|
|
#if CH_DBG_ENABLE_STACK_CHECK == TRUE
|
|
|
|
static void stackCheck(uint16_t& mstack, uint16_t& pstack) {
|
2024-01-15 16:32:17 -04:00
|
|
|
extern stkalign_t __main_stack_base__[];
|
|
|
|
extern stkalign_t __main_stack_end__[];
|
2023-05-24 17:43:17 -03:00
|
|
|
uint32_t stklimit = (uint32_t)__main_stack_end__;
|
|
|
|
uint32_t stkbase = (uint32_t)__main_stack_base__;
|
|
|
|
uint32_t *crawl = (uint32_t *)stkbase;
|
|
|
|
|
|
|
|
while (*crawl == 0x55555555 && crawl < (uint32_t *)stklimit) {
|
|
|
|
crawl++;
|
|
|
|
}
|
|
|
|
uint32_t free = (uint32_t)crawl - stkbase;
|
|
|
|
chDbgAssert(free > 0, "mstack exhausted");
|
|
|
|
mstack = (uint16_t)free;
|
|
|
|
|
2024-01-15 16:32:17 -04:00
|
|
|
extern stkalign_t __main_thread_stack_base__[];
|
|
|
|
extern stkalign_t __main_thread_stack_end__[];
|
2023-05-24 17:43:17 -03:00
|
|
|
stklimit = (uint32_t)__main_thread_stack_end__;
|
|
|
|
stkbase = (uint32_t)__main_thread_stack_base__;
|
|
|
|
crawl = (uint32_t *)stkbase;
|
|
|
|
|
|
|
|
while (*crawl == 0x55555555 && crawl < (uint32_t *)stklimit) {
|
|
|
|
crawl++;
|
|
|
|
}
|
|
|
|
free = (uint32_t)crawl - stkbase;
|
|
|
|
chDbgAssert(free > 0, "pstack exhausted");
|
|
|
|
pstack = (uint16_t)free;
|
|
|
|
}
|
|
|
|
#endif /* CH_DBG_ENABLE_STACK_CHECK == TRUE */
|
|
|
|
|
2023-06-09 17:42:29 -03:00
|
|
|
/*
|
|
|
|
Update loop design.
|
|
|
|
|
|
|
|
Considerations - the F100 is quite slow and so processing time needs to be used effectively.
|
|
|
|
The CPU time slices required by dshot are generally faster than those required for other processing.
|
|
|
|
Dshot requires even updates at at least 1Khz and generally faster if SERVO_DSHOT_RATE is used.
|
|
|
|
The two most time sensitive regular functions are (1) PWM updates which run at loop rate triggered from the FMU
|
|
|
|
(and thus require efficient code page write) and (2) rcin updates which run at a fixed 1Khz cycle (a speed
|
|
|
|
which is assumed by the rcin protocol handlers) and require efficient code read. The FMU sends code page
|
|
|
|
requests which require a response within 10ms in order to prevent the IOMCU being considered to have failed,
|
|
|
|
however code page requests are always initiated by the FMU and so the IOMCU only ever needs to be ready
|
|
|
|
to read requests - writing responses are always in response to a request. Finally, PWM channels 3-4 share a DMA
|
|
|
|
channel with UART TX and so access needs to be mediated.
|
|
|
|
|
|
|
|
Design -
|
|
|
|
1. requests are read using circular DMA. In other words the RX side of the UART is always ready. Once
|
|
|
|
a request has been processed DMA is immediately set up for a new request.
|
|
|
|
2. responses are only ever sent in response to a request. As soon as a request is received the ISR only
|
|
|
|
ever requests that a response be sent - it never actually sends a response.
|
|
|
|
3. The update loop waits for four different events:
|
|
|
|
3a - a request has been received and should be processed. This does not require the TX DMA lock.
|
|
|
|
3b - a response needs to be sent. This requires the TX DMA lock.
|
|
|
|
3c - a response has been sent. This allows the TX DMA lock to be released.
|
|
|
|
3d - an out of band PWM request, usually triggered by a failsafe needs to be processed.
|
|
|
|
Since requests are processed continuously it is possible for 3b and 3c to occur simultaneously. Since the
|
|
|
|
TX lock is already held to send the previous response, there is no need to do anything with the lock in order
|
|
|
|
to process the next response.
|
|
|
|
|
|
|
|
Profiling shows that sending a response takes very little time - 10s of microseconds - and so a response is sent
|
|
|
|
if required at the beginning of the update. This means that by the end of the update there is a very high chance
|
|
|
|
that the response will have already been sent and this is therefore checked. If the response has been sent the
|
|
|
|
lock is released. If for some reason the response has not gone out, as soon as it does an event will be posted
|
|
|
|
and the update loop will run again.
|
|
|
|
|
|
|
|
This design means that on average the update loop is idle with the TX DMA channel unlocked. This maximises the
|
|
|
|
time that dshot can run uninterrupted leading to very efficient and even output.
|
|
|
|
|
|
|
|
Finally the update loop has a timeout which forces updates to progress even in the absence of requests from the
|
|
|
|
FMU. Since responses will always be triggered in a timely fashion, regardlesss of the timeout, this can be
|
|
|
|
set relatively long.
|
|
|
|
|
|
|
|
If compiled without sharing, DMA - and thus dshot - is not used on channels 3-4, there are no locks and responses
|
|
|
|
are always setup in the request ISR handler.
|
|
|
|
*/
|
2018-05-05 15:16:52 -03:00
|
|
|
void AP_IOMCU_FW::update()
|
|
|
|
{
|
2023-06-09 17:42:29 -03:00
|
|
|
eventmask_t mask = chEvtWaitAnyTimeout(IOEVENT_PWM | IOEVENT_TX_END | IOEVENT_TX_BEGIN, TIME_US2I(1000));
|
2023-06-23 17:15:14 -03:00
|
|
|
#ifdef HAL_GPIO_LINE_GPIO107
|
2023-06-01 12:06:52 -03:00
|
|
|
TOGGLE_PIN_DEBUG(107);
|
2023-06-23 17:15:14 -03:00
|
|
|
#endif
|
2023-06-01 12:06:52 -03:00
|
|
|
|
|
|
|
iomcu.reg_status.total_ticks++;
|
|
|
|
if (mask) {
|
|
|
|
iomcu.reg_status.total_events++;
|
|
|
|
}
|
|
|
|
|
|
|
|
#if AP_HAL_SHARED_DMA_ENABLED
|
2023-06-09 17:42:29 -03:00
|
|
|
// See discussion above
|
|
|
|
if ((mask & IOEVENT_TX_BEGIN) && !(mask & IOEVENT_TX_END)) { // 3b - lock required to send response
|
|
|
|
tx_dma_handle->lock();
|
|
|
|
} else if (!(mask & IOEVENT_TX_BEGIN) && (mask & IOEVENT_TX_END)) { // 3c - response sent, lock can be released
|
|
|
|
tx_dma_handle->unlock();
|
|
|
|
} // else 3b and 3c - current lock required for new response
|
|
|
|
|
|
|
|
// send a response if required
|
|
|
|
if (mask & IOEVENT_TX_BEGIN) {
|
|
|
|
chSysLock();
|
|
|
|
setup_tx_dma(&UARTD2);
|
2023-06-01 12:06:52 -03:00
|
|
|
chSysUnlock();
|
|
|
|
}
|
2023-05-24 17:43:17 -03:00
|
|
|
#endif
|
2018-05-11 03:31:04 -03:00
|
|
|
|
2018-10-31 19:06:08 -03:00
|
|
|
// we get the timestamp once here, and avoid fetching it
|
|
|
|
// within the DMA callbacks
|
|
|
|
last_ms = AP_HAL::millis();
|
|
|
|
loop_counter++;
|
|
|
|
|
|
|
|
if (do_reboot && (last_ms > reboot_time)) {
|
2018-05-05 15:16:52 -03:00
|
|
|
hal.scheduler->reboot(true);
|
2018-09-14 07:06:59 -03:00
|
|
|
while (true) {}
|
2018-05-05 15:16:52 -03:00
|
|
|
}
|
2023-06-01 12:06:52 -03:00
|
|
|
if ((mask & IOEVENT_PWM) ||
|
2018-05-11 05:11:24 -03:00
|
|
|
(last_safety_off != reg_status.flag_safety_off)) {
|
|
|
|
last_safety_off = reg_status.flag_safety_off;
|
|
|
|
pwm_out_update();
|
|
|
|
}
|
2018-05-11 03:31:04 -03:00
|
|
|
|
2018-10-31 19:06:08 -03:00
|
|
|
uint32_t now = last_ms;
|
2023-06-01 12:06:52 -03:00
|
|
|
uint32_t now_us = AP_HAL::micros();
|
2018-10-29 02:51:43 -03:00
|
|
|
|
2023-06-01 12:06:52 -03:00
|
|
|
reg_status.timestamp_ms = last_ms;
|
2018-10-29 02:51:43 -03:00
|
|
|
// output SBUS if enabled
|
|
|
|
if ((reg_setup.features & P_SETUP_FEATURES_SBUS1_OUT) &&
|
|
|
|
reg_status.flag_safety_off &&
|
|
|
|
now - sbus_last_ms >= sbus_interval_ms) {
|
|
|
|
// output a new SBUS frame
|
|
|
|
sbus_last_ms = now;
|
2023-11-13 15:58:36 -04:00
|
|
|
sbus_out_write(reg_servo.pwm, IOMCU_MAX_RC_CHANNELS);
|
2018-10-29 02:51:43 -03:00
|
|
|
}
|
2018-10-30 23:10:51 -03:00
|
|
|
// handle FMU failsafe
|
2018-10-31 19:31:45 -03:00
|
|
|
if (now - fmu_data_received_time > 200) {
|
|
|
|
// we are not getting input from the FMU. Fill in failsafe values at 100Hz
|
|
|
|
if (now - last_failsafe_ms > 10) {
|
|
|
|
fill_failsafe_pwm();
|
2023-06-01 12:06:52 -03:00
|
|
|
chEvtSignal(thread_ctx, IOEVENT_PWM);
|
2018-10-31 19:31:45 -03:00
|
|
|
last_failsafe_ms = now;
|
|
|
|
}
|
2018-11-01 04:24:41 -03:00
|
|
|
// turn amber on
|
|
|
|
AMBER_SET(1);
|
2018-10-31 19:31:45 -03:00
|
|
|
} else {
|
|
|
|
last_failsafe_ms = now;
|
2018-11-01 04:24:41 -03:00
|
|
|
// turn amber off
|
|
|
|
AMBER_SET(0);
|
2018-10-30 23:10:51 -03:00
|
|
|
}
|
2023-06-23 17:15:14 -03:00
|
|
|
|
2018-10-31 19:06:08 -03:00
|
|
|
// update status page at 20Hz
|
|
|
|
if (now - last_status_ms > 50) {
|
|
|
|
last_status_ms = now;
|
|
|
|
page_status_update();
|
|
|
|
}
|
2023-06-23 17:15:14 -03:00
|
|
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
|
|
|
// EDT updates are semt at ~1Hz per ESC, but we want to make sure
|
|
|
|
// that we don't delay updates unduly so sample at 5Hz
|
|
|
|
if (now - last_telem_ms > 200) {
|
|
|
|
last_telem_ms = now;
|
|
|
|
telem_update();
|
|
|
|
}
|
|
|
|
#endif
|
2023-06-01 12:06:52 -03:00
|
|
|
// run fast loop functions at 1Khz
|
2023-06-24 15:16:43 -03:00
|
|
|
if (now_us - last_fast_loop_us >= 1000)
|
|
|
|
{
|
2023-06-01 12:06:52 -03:00
|
|
|
last_fast_loop_us = now_us;
|
2023-08-14 04:43:33 -03:00
|
|
|
heater_update();
|
2018-05-11 03:31:04 -03:00
|
|
|
rcin_update();
|
2023-06-01 12:06:52 -03:00
|
|
|
rcin_serial_update();
|
2023-06-23 17:15:14 -03:00
|
|
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
|
|
|
erpm_update();
|
|
|
|
#endif
|
2023-06-01 12:06:52 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// run remaining functions at 100Hz
|
|
|
|
// these are all relatively expensive and take ~10ms to complete
|
|
|
|
// so there is no way they can effectively be run faster than 100Hz
|
|
|
|
if (now - last_slow_loop_ms > 10) {
|
|
|
|
last_slow_loop_ms = now;
|
2018-05-11 03:31:04 -03:00
|
|
|
safety_update();
|
2023-05-24 17:43:17 -03:00
|
|
|
rcout_config_update();
|
2018-05-11 03:31:04 -03:00
|
|
|
hal.rcout->timer_tick();
|
2018-10-31 19:24:18 -03:00
|
|
|
if (dsm_bind_state) {
|
|
|
|
dsm_bind_step();
|
|
|
|
}
|
2021-09-20 10:43:32 -03:00
|
|
|
GPIO_write();
|
2023-05-24 17:43:17 -03:00
|
|
|
#if CH_DBG_ENABLE_STACK_CHECK == TRUE
|
|
|
|
stackCheck(reg_status.freemstack, reg_status.freepstack);
|
|
|
|
#endif
|
2018-05-11 03:31:04 -03:00
|
|
|
}
|
2023-06-01 12:06:52 -03:00
|
|
|
#if AP_HAL_SHARED_DMA_ENABLED
|
2023-06-09 17:42:29 -03:00
|
|
|
// check whether a response has now been sent
|
|
|
|
mask = chEvtGetAndClearEvents(IOEVENT_TX_END);
|
2023-06-01 12:06:52 -03:00
|
|
|
|
2023-06-09 17:42:29 -03:00
|
|
|
if (mask) {
|
|
|
|
tx_dma_handle->unlock();
|
|
|
|
}
|
2023-06-01 12:06:52 -03:00
|
|
|
#endif
|
2023-06-23 17:15:14 -03:00
|
|
|
#ifdef HAL_GPIO_LINE_GPIO107
|
2023-06-01 12:06:52 -03:00
|
|
|
TOGGLE_PIN_DEBUG(107);
|
2023-06-23 17:15:14 -03:00
|
|
|
#endif
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AP_IOMCU_FW::pwm_out_update()
|
|
|
|
{
|
|
|
|
memcpy(reg_servo.pwm, reg_direct_pwm.pwm, sizeof(reg_direct_pwm));
|
2018-05-11 03:31:04 -03:00
|
|
|
hal.rcout->cork();
|
2018-09-14 07:06:59 -03:00
|
|
|
for (uint8_t i = 0; i < SERVO_COUNT; i++) {
|
2018-10-31 01:50:31 -03:00
|
|
|
if (reg_status.flag_safety_off || (reg_setup.ignore_safety & (1U<<i))) {
|
|
|
|
hal.rcout->write(i, reg_servo.pwm[i]);
|
|
|
|
} else {
|
|
|
|
hal.rcout->write(i, 0);
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
|
|
|
}
|
2018-05-11 03:31:04 -03:00
|
|
|
hal.rcout->push();
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AP_IOMCU_FW::heater_update()
|
|
|
|
{
|
2018-10-31 19:06:08 -03:00
|
|
|
uint32_t now = last_ms;
|
2018-05-10 04:12:40 -03:00
|
|
|
if (!has_heater) {
|
2018-10-31 19:06:08 -03:00
|
|
|
// use blue LED as heartbeat, run it 4x faster when override active
|
|
|
|
if (now - last_blue_led_ms > (override_active?125:500)) {
|
2018-11-01 04:24:41 -03:00
|
|
|
BLUE_TOGGLE();
|
2018-05-10 04:12:40 -03:00
|
|
|
last_blue_led_ms = now;
|
|
|
|
}
|
|
|
|
} else if (reg_setup.heater_duty_cycle == 0 || (now - last_heater_ms > 3000UL)) {
|
2018-11-01 04:24:41 -03:00
|
|
|
// turn off the heater
|
2019-05-06 07:39:47 -03:00
|
|
|
HEATER_SET(!heater_pwm_polarity);
|
2018-09-14 07:06:59 -03:00
|
|
|
} else {
|
2019-07-11 20:42:21 -03:00
|
|
|
// we use a pseudo random sequence to dither the cycling as
|
|
|
|
// the heater has a significant effect on the internal
|
|
|
|
// magnetometers. The random generator dithers this so we don't get a 1Hz cycly in the magnetometer.
|
|
|
|
// The impact on the mags is about 25 mGauss.
|
|
|
|
bool heater_on = (get_random16() < uint32_t(reg_setup.heater_duty_cycle) * 0xFFFFU / 100U);
|
|
|
|
HEATER_SET(heater_on? heater_pwm_polarity : !heater_pwm_polarity);
|
2018-09-14 07:06:59 -03:00
|
|
|
}
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AP_IOMCU_FW::rcin_update()
|
|
|
|
{
|
2018-10-05 21:33:30 -03:00
|
|
|
((ChibiOS::RCInput *)hal.rcin)->_timer_tick();
|
2018-05-02 08:22:17 -03:00
|
|
|
if (hal.rcin->new_input()) {
|
2022-03-16 07:03:09 -03:00
|
|
|
const auto &rc = AP::RC();
|
2018-05-02 08:22:17 -03:00
|
|
|
rc_input.count = hal.rcin->num_channels();
|
|
|
|
rc_input.flags_rc_ok = true;
|
2023-11-13 15:58:36 -04:00
|
|
|
hal.rcin->read(rc_input.pwm, IOMCU_MAX_RC_CHANNELS);
|
2019-08-13 21:07:48 -03:00
|
|
|
rc_last_input_ms = last_ms;
|
2022-03-16 07:03:09 -03:00
|
|
|
rc_input.rc_protocol = (uint16_t)rc.protocol_detected();
|
|
|
|
rc_input.rssi = rc.get_RSSI();
|
|
|
|
rc_input.flags_failsafe = rc.failsafe_active();
|
2019-08-13 21:07:48 -03:00
|
|
|
} else if (last_ms - rc_last_input_ms > 200U) {
|
2018-10-05 22:04:13 -03:00
|
|
|
rc_input.flags_rc_ok = false;
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
2018-05-05 15:16:52 -03:00
|
|
|
if (update_rcout_freq) {
|
|
|
|
hal.rcout->set_freq(reg_setup.pwm_rates, reg_setup.pwm_altrate);
|
|
|
|
update_rcout_freq = false;
|
|
|
|
}
|
|
|
|
if (update_default_rate) {
|
|
|
|
hal.rcout->set_default_rate(reg_setup.pwm_defaultrate);
|
2020-01-28 22:21:03 -04:00
|
|
|
update_default_rate = false;
|
2018-05-05 15:16:52 -03:00
|
|
|
}
|
|
|
|
|
2018-11-05 21:02:55 -04:00
|
|
|
bool old_override = override_active;
|
|
|
|
|
2018-10-30 23:10:51 -03:00
|
|
|
// check for active override channel
|
|
|
|
if (mixing.enabled &&
|
|
|
|
mixing.rc_chan_override > 0 &&
|
2018-11-01 21:03:33 -03:00
|
|
|
rc_input.flags_rc_ok &&
|
2023-11-13 15:58:36 -04:00
|
|
|
mixing.rc_chan_override <= IOMCU_MAX_RC_CHANNELS) {
|
2018-10-31 00:58:01 -03:00
|
|
|
override_active = (rc_input.pwm[mixing.rc_chan_override-1] >= 1750);
|
2018-10-30 23:10:51 -03:00
|
|
|
} else {
|
|
|
|
override_active = false;
|
|
|
|
}
|
2018-11-05 21:02:55 -04:00
|
|
|
if (old_override != override_active) {
|
|
|
|
if (override_active) {
|
|
|
|
fill_failsafe_pwm();
|
|
|
|
}
|
2023-06-01 12:06:52 -03:00
|
|
|
chEvtSignal(thread_ctx, IOEVENT_PWM);
|
2018-11-05 21:02:55 -04:00
|
|
|
}
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
|
|
|
|
2023-06-23 17:15:14 -03:00
|
|
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
|
|
|
void AP_IOMCU_FW::erpm_update()
|
|
|
|
{
|
2023-10-23 10:36:02 -03:00
|
|
|
uint32_t now_us = AP_HAL::micros();
|
|
|
|
|
2023-06-23 17:15:14 -03:00
|
|
|
if (hal.rcout->new_erpm()) {
|
2023-11-13 15:58:36 -04:00
|
|
|
dshot_erpm.update_mask |= hal.rcout->read_erpm(dshot_erpm.erpm, IOMCU_MAX_TELEM_CHANNELS);
|
2023-10-23 10:36:02 -03:00
|
|
|
last_erpm_us = now_us;
|
|
|
|
} else if (now_us - last_erpm_us > ESC_RPM_DATA_TIMEOUT_US) {
|
|
|
|
dshot_erpm.update_mask = 0;
|
2023-06-23 17:15:14 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_IOMCU_FW::telem_update()
|
|
|
|
{
|
|
|
|
uint32_t now_ms = AP_HAL::millis();
|
|
|
|
|
2023-11-13 15:58:36 -04:00
|
|
|
for (uint8_t i = 0; i < IOMCU_MAX_TELEM_CHANNELS/4; i++) {
|
2024-05-13 20:16:23 -03:00
|
|
|
struct page_dshot_telem &dshot_i = dshot_telem[i];
|
2023-06-23 17:15:14 -03:00
|
|
|
for (uint8_t j = 0; j < 4; j++) {
|
|
|
|
const uint8_t esc_id = (i * 4 + j);
|
2023-11-13 15:58:36 -04:00
|
|
|
if (esc_id >= IOMCU_MAX_TELEM_CHANNELS) {
|
2023-06-23 17:15:14 -03:00
|
|
|
continue;
|
|
|
|
}
|
2024-05-13 20:16:23 -03:00
|
|
|
dshot_i.error_rate[j] = uint16_t(roundf(hal.rcout->get_erpm_error_rate(esc_id) * 100.0));
|
2023-06-23 17:15:14 -03:00
|
|
|
#if HAL_WITH_ESC_TELEM
|
|
|
|
const volatile AP_ESC_Telem_Backend::TelemetryData& telem = esc_telem.get_telem_data(esc_id);
|
2023-10-23 10:36:02 -03:00
|
|
|
// if data is stale then set to zero to avoid phantom data appearing in mavlink
|
2023-06-23 17:15:14 -03:00
|
|
|
if (now_ms - telem.last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS) {
|
2024-05-13 20:16:23 -03:00
|
|
|
dshot_i.voltage_cvolts[j] = 0;
|
|
|
|
dshot_i.current_camps[j] = 0;
|
|
|
|
dshot_i.temperature_cdeg[j] = 0;
|
|
|
|
#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
|
|
|
|
dshot_i.edt2_status[j] = 0;
|
|
|
|
dshot_i.edt2_stress[j] = 0;
|
|
|
|
#endif
|
2023-06-23 17:15:14 -03:00
|
|
|
continue;
|
|
|
|
}
|
2024-05-13 20:16:23 -03:00
|
|
|
dshot_i.voltage_cvolts[j] = uint16_t(roundf(telem.voltage * 100));
|
|
|
|
dshot_i.current_camps[j] = uint16_t(roundf(telem.current * 100));
|
|
|
|
dshot_i.temperature_cdeg[j] = telem.temperature_cdeg;
|
|
|
|
#if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED
|
|
|
|
dshot_i.edt2_status[j] = uint8_t(telem.edt2_status);
|
|
|
|
dshot_i.edt2_stress[j] = uint8_t(telem.edt2_stress);
|
|
|
|
#endif
|
|
|
|
dshot_i.types[j] = telem.types;
|
2023-06-23 17:15:14 -03:00
|
|
|
#endif
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
void AP_IOMCU_FW::process_io_packet()
|
|
|
|
{
|
2019-08-13 21:07:48 -03:00
|
|
|
iomcu.reg_status.total_pkts++;
|
|
|
|
|
2023-12-21 11:06:36 -04:00
|
|
|
if (rx_io_packet.code == CODE_NOOP) {
|
|
|
|
iomcu.reg_status.num_errors++;
|
|
|
|
iomcu.reg_status.err_bad_opcode++;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
uint8_t rx_crc = rx_io_packet.crc;
|
2018-10-30 21:24:51 -03:00
|
|
|
uint8_t calc_crc;
|
2018-05-02 08:22:17 -03:00
|
|
|
rx_io_packet.crc = 0;
|
2018-10-30 21:24:51 -03:00
|
|
|
uint8_t pkt_size = rx_io_packet.get_size();
|
|
|
|
if (rx_io_packet.code == CODE_READ) {
|
|
|
|
// allow for more bandwidth efficient read packets
|
|
|
|
calc_crc = crc_crc8((const uint8_t *)&rx_io_packet, 4);
|
|
|
|
if (calc_crc != rx_crc) {
|
|
|
|
calc_crc = crc_crc8((const uint8_t *)&rx_io_packet, pkt_size);
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
calc_crc = crc_crc8((const uint8_t *)&rx_io_packet, pkt_size);
|
|
|
|
}
|
2018-10-31 19:06:08 -03:00
|
|
|
if (rx_crc != calc_crc || rx_io_packet.count > PKT_MAX_REGS) {
|
2018-05-02 08:22:17 -03:00
|
|
|
tx_io_packet.count = 0;
|
|
|
|
tx_io_packet.code = CODE_CORRUPT;
|
|
|
|
tx_io_packet.crc = 0;
|
2018-11-26 06:46:22 -04:00
|
|
|
tx_io_packet.page = 0;
|
|
|
|
tx_io_packet.offset = 0;
|
2018-05-02 08:22:17 -03:00
|
|
|
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
|
2019-08-13 21:07:48 -03:00
|
|
|
iomcu.reg_status.num_errors++;
|
|
|
|
iomcu.reg_status.err_crc++;
|
2018-05-02 08:22:17 -03:00
|
|
|
return;
|
|
|
|
}
|
2018-09-14 07:06:59 -03:00
|
|
|
switch (rx_io_packet.code) {
|
|
|
|
case CODE_READ: {
|
|
|
|
if (!handle_code_read()) {
|
|
|
|
tx_io_packet.count = 0;
|
|
|
|
tx_io_packet.code = CODE_ERROR;
|
|
|
|
tx_io_packet.crc = 0;
|
2018-11-26 06:46:22 -04:00
|
|
|
tx_io_packet.page = 0;
|
|
|
|
tx_io_packet.offset = 0;
|
2018-09-14 07:06:59 -03:00
|
|
|
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
|
2019-08-13 21:07:48 -03:00
|
|
|
iomcu.reg_status.num_errors++;
|
|
|
|
iomcu.reg_status.err_read++;
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
2018-09-14 07:06:59 -03:00
|
|
|
}
|
|
|
|
break;
|
|
|
|
case CODE_WRITE: {
|
|
|
|
if (!handle_code_write()) {
|
|
|
|
tx_io_packet.count = 0;
|
|
|
|
tx_io_packet.code = CODE_ERROR;
|
|
|
|
tx_io_packet.crc = 0;
|
2018-11-26 06:46:22 -04:00
|
|
|
tx_io_packet.page = 0;
|
|
|
|
tx_io_packet.offset = 0;
|
2018-09-14 07:06:59 -03:00
|
|
|
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
|
2019-08-13 21:07:48 -03:00
|
|
|
iomcu.reg_status.num_errors++;
|
|
|
|
iomcu.reg_status.err_write++;
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
2018-09-14 07:06:59 -03:00
|
|
|
}
|
|
|
|
break;
|
|
|
|
default: {
|
2019-08-13 21:07:48 -03:00
|
|
|
iomcu.reg_status.num_errors++;
|
|
|
|
iomcu.reg_status.err_bad_opcode++;
|
2023-12-21 11:06:36 -04:00
|
|
|
rx_io_packet.code = CODE_NOOP;
|
|
|
|
rx_io_packet.count = 0;
|
|
|
|
return;
|
2018-09-14 07:06:59 -03:00
|
|
|
}
|
|
|
|
break;
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
2023-12-21 11:06:36 -04:00
|
|
|
|
|
|
|
// prevent a spurious DMA callback from doing anything bad
|
|
|
|
rx_io_packet.code = CODE_NOOP;
|
|
|
|
rx_io_packet.count = 0;
|
|
|
|
|
|
|
|
return;
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
|
|
|
|
2018-10-28 20:23:31 -03:00
|
|
|
/*
|
|
|
|
update dynamic elements of status page
|
|
|
|
*/
|
|
|
|
void AP_IOMCU_FW::page_status_update(void)
|
|
|
|
{
|
2023-12-09 09:54:26 -04:00
|
|
|
adc_sample_channels();
|
|
|
|
|
2018-10-29 02:51:43 -03:00
|
|
|
if ((reg_setup.features & P_SETUP_FEATURES_SBUS1_OUT) == 0) {
|
|
|
|
// we can only get VRSSI when sbus is disabled
|
2023-12-09 09:54:26 -04:00
|
|
|
reg_status.vrssi = adc_vrssi();
|
2018-10-29 02:51:43 -03:00
|
|
|
} else {
|
|
|
|
reg_status.vrssi = 0;
|
|
|
|
}
|
2023-12-09 09:54:26 -04:00
|
|
|
reg_status.vservo = adc_vservo();
|
2018-10-28 20:23:31 -03:00
|
|
|
}
|
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
bool AP_IOMCU_FW::handle_code_read()
|
|
|
|
{
|
2018-09-14 07:06:59 -03:00
|
|
|
uint16_t *values = nullptr;
|
|
|
|
#define COPY_PAGE(_page_name) \
|
2018-05-02 08:22:17 -03:00
|
|
|
do { \
|
|
|
|
values = (uint16_t *)&_page_name; \
|
|
|
|
tx_io_packet.count = sizeof(_page_name) / sizeof(uint16_t); \
|
|
|
|
} while(0);
|
|
|
|
|
2018-09-14 07:06:59 -03:00
|
|
|
switch (rx_io_packet.page) {
|
2018-10-30 21:07:47 -03:00
|
|
|
case PAGE_CONFIG:
|
|
|
|
COPY_PAGE(config);
|
|
|
|
break;
|
2018-09-14 07:06:59 -03:00
|
|
|
case PAGE_SETUP:
|
|
|
|
COPY_PAGE(reg_setup);
|
|
|
|
break;
|
|
|
|
case PAGE_RAW_RCIN:
|
|
|
|
COPY_PAGE(rc_input);
|
|
|
|
break;
|
2023-06-23 17:15:14 -03:00
|
|
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
|
|
|
case PAGE_RAW_DSHOT_ERPM:
|
|
|
|
COPY_PAGE(dshot_erpm);
|
|
|
|
break;
|
|
|
|
case PAGE_RAW_DSHOT_TELEM_1_4:
|
|
|
|
COPY_PAGE(dshot_telem[0]);
|
|
|
|
break;
|
2023-11-13 15:58:36 -04:00
|
|
|
#if IOMCU_MAX_TELEM_CHANNELS > 4
|
2023-06-23 17:15:14 -03:00
|
|
|
case PAGE_RAW_DSHOT_TELEM_5_8:
|
|
|
|
COPY_PAGE(dshot_telem[1]);
|
|
|
|
break;
|
2023-11-13 15:58:36 -04:00
|
|
|
#endif
|
2023-06-23 17:15:14 -03:00
|
|
|
#endif
|
2018-09-14 07:06:59 -03:00
|
|
|
case PAGE_STATUS:
|
|
|
|
COPY_PAGE(reg_status);
|
|
|
|
break;
|
|
|
|
case PAGE_SERVOS:
|
|
|
|
COPY_PAGE(reg_servo);
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
return false;
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
|
|
|
|
2018-09-14 07:06:59 -03:00
|
|
|
/* if the offset is at or beyond the end of the page, we have no data */
|
2018-10-31 19:06:08 -03:00
|
|
|
if (rx_io_packet.offset + rx_io_packet.count > tx_io_packet.count) {
|
2018-09-14 07:06:59 -03:00
|
|
|
return false;
|
|
|
|
}
|
2018-05-02 08:22:17 -03:00
|
|
|
|
2018-09-14 07:06:59 -03:00
|
|
|
/* correct the data pointer and count for the offset */
|
|
|
|
values += rx_io_packet.offset;
|
2018-11-01 03:39:24 -03:00
|
|
|
tx_io_packet.page = rx_io_packet.page;
|
|
|
|
tx_io_packet.offset = rx_io_packet.offset;
|
2018-09-14 07:06:59 -03:00
|
|
|
tx_io_packet.count -= rx_io_packet.offset;
|
2018-10-29 18:54:43 -03:00
|
|
|
tx_io_packet.count = MIN(tx_io_packet.count, rx_io_packet.count);
|
2018-10-31 19:06:08 -03:00
|
|
|
tx_io_packet.count = MIN(tx_io_packet.count, PKT_MAX_REGS);
|
2018-11-01 03:39:24 -03:00
|
|
|
tx_io_packet.code = CODE_SUCCESS;
|
2018-05-02 08:22:17 -03:00
|
|
|
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());
|
2023-06-23 17:15:14 -03:00
|
|
|
|
|
|
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
|
|
|
switch (rx_io_packet.page) {
|
|
|
|
case PAGE_RAW_DSHOT_ERPM:
|
|
|
|
memset(&dshot_erpm, 0, sizeof(dshot_erpm));
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
#endif
|
2018-05-02 08:22:17 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
bool AP_IOMCU_FW::handle_code_write()
|
|
|
|
{
|
2018-09-14 07:06:59 -03:00
|
|
|
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) {
|
|
|
|
hal.rcout->force_safety_off();
|
|
|
|
reg_status.flag_safety_off = true;
|
|
|
|
} else {
|
|
|
|
return false;
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
2018-09-14 07:06:59 -03:00
|
|
|
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;
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
2018-09-14 07:06:59 -03:00
|
|
|
break;
|
|
|
|
case PAGE_REG_SETUP_ALTRATE:
|
|
|
|
reg_setup.pwm_altrate = rx_io_packet.regs[0];
|
|
|
|
update_rcout_freq = true;
|
|
|
|
break;
|
|
|
|
case PAGE_REG_SETUP_PWM_RATE_MASK:
|
|
|
|
reg_setup.pwm_rates = rx_io_packet.regs[0];
|
|
|
|
update_rcout_freq = true;
|
|
|
|
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];
|
|
|
|
update_default_rate = true;
|
|
|
|
break;
|
2023-05-24 17:43:17 -03:00
|
|
|
case PAGE_REG_SETUP_DSHOT_PERIOD:
|
|
|
|
reg_setup.dshot_period_us = rx_io_packet.regs[0];
|
|
|
|
reg_setup.dshot_rate = rx_io_packet.regs[1];
|
|
|
|
hal.rcout->set_dshot_period(reg_setup.dshot_period_us, reg_setup.dshot_rate);
|
|
|
|
break;
|
|
|
|
case PAGE_REG_SETUP_CHANNEL_MASK:
|
|
|
|
reg_setup.channel_mask = rx_io_packet.regs[0];
|
|
|
|
break;
|
2018-09-14 07:06:59 -03:00
|
|
|
case PAGE_REG_SETUP_SBUS_RATE:
|
2018-10-29 02:51:43 -03:00
|
|
|
reg_setup.sbus_rate = rx_io_packet.regs[0];
|
|
|
|
sbus_interval_ms = MAX(1000U / reg_setup.sbus_rate,3);
|
2018-09-14 07:06:59 -03:00
|
|
|
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);
|
2018-10-29 02:51:43 -03:00
|
|
|
|
|
|
|
// enable SBUS output at specified rate
|
|
|
|
sbus_interval_ms = MAX(1000U / reg_setup.sbus_rate,3);
|
2018-10-31 21:59:54 -03:00
|
|
|
|
|
|
|
// we need to release the JTAG reset pin to be used as a GPIO, otherwise we can't enable
|
|
|
|
// or disable SBUS out
|
|
|
|
AFIO->MAPR = AFIO_MAPR_SWJ_CFG_NOJNTRST;
|
|
|
|
|
2023-12-09 09:54:26 -04:00
|
|
|
adc_disable_vrssi();
|
2018-10-29 02:51:43 -03:00
|
|
|
palClearLine(HAL_GPIO_PIN_SBUS_OUT_EN);
|
|
|
|
} else {
|
2023-12-09 09:54:26 -04:00
|
|
|
adc_enable_vrssi();
|
2018-10-29 02:51:43 -03:00
|
|
|
palSetLine(HAL_GPIO_PIN_SBUS_OUT_EN);
|
2018-09-14 07:06:59 -03:00
|
|
|
}
|
2023-03-01 00:05:02 -04:00
|
|
|
if (reg_setup.features & P_SETUP_FEATURES_HEATER) {
|
|
|
|
has_heater = true;
|
|
|
|
}
|
2018-09-14 07:06:59 -03:00
|
|
|
break;
|
2018-10-31 19:24:18 -03:00
|
|
|
|
2020-07-11 09:22:05 -03:00
|
|
|
case PAGE_REG_SETUP_OUTPUT_MODE:
|
|
|
|
mode_out.mask = rx_io_packet.regs[0];
|
|
|
|
mode_out.mode = rx_io_packet.regs[1];
|
2023-06-23 17:15:14 -03:00
|
|
|
mode_out.bdmask = rx_io_packet.regs[2];
|
|
|
|
mode_out.esc_type = rx_io_packet.regs[3];
|
2024-08-13 09:59:08 -03:00
|
|
|
mode_out.reversible_mask = rx_io_packet.regs[4];
|
2020-07-11 09:22:05 -03:00
|
|
|
break;
|
|
|
|
|
2018-09-14 07:06:59 -03:00
|
|
|
case PAGE_REG_SETUP_HEATER_DUTY_CYCLE:
|
|
|
|
reg_setup.heater_duty_cycle = rx_io_packet.regs[0];
|
2018-10-31 19:06:08 -03:00
|
|
|
last_heater_ms = last_ms;
|
2018-09-14 07:06:59 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case PAGE_REG_SETUP_REBOOT_BL:
|
|
|
|
if (reg_status.flag_safety_off) {
|
|
|
|
// don't allow reboot while armed
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check the magic value
|
|
|
|
if (rx_io_packet.regs[0] != REBOOT_BL_MAGIC) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
schedule_reboot(100);
|
|
|
|
break;
|
|
|
|
|
2018-10-31 01:50:31 -03:00
|
|
|
case PAGE_REG_SETUP_IGNORE_SAFETY:
|
|
|
|
reg_setup.ignore_safety = rx_io_packet.regs[0];
|
|
|
|
((ChibiOS::RCOutput *)hal.rcout)->set_safety_mask(reg_setup.ignore_safety);
|
|
|
|
break;
|
2018-10-31 19:24:18 -03:00
|
|
|
|
|
|
|
case PAGE_REG_SETUP_DSM_BIND:
|
2018-10-31 19:54:18 -03:00
|
|
|
if (dsm_bind_state == 0) {
|
|
|
|
dsm_bind_state = 1;
|
|
|
|
}
|
2018-10-31 19:24:18 -03:00
|
|
|
break;
|
2019-10-20 10:47:14 -03:00
|
|
|
|
2020-08-12 23:28:47 -03:00
|
|
|
case PAGE_REG_SETUP_RC_PROTOCOLS: {
|
|
|
|
if (rx_io_packet.count == 2) {
|
|
|
|
uint32_t v;
|
|
|
|
memcpy(&v, &rx_io_packet.regs[0], 4);
|
|
|
|
AP::RC().set_rc_protocols(v);
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
default:
|
|
|
|
break;
|
2018-09-14 07:06:59 -03:00
|
|
|
}
|
|
|
|
break;
|
2018-10-30 23:10:51 -03:00
|
|
|
|
2018-09-14 07:06:59 -03:00
|
|
|
case PAGE_DIRECT_PWM: {
|
2018-10-30 23:10:51 -03:00
|
|
|
if (override_active) {
|
|
|
|
// no input when override is active
|
|
|
|
break;
|
|
|
|
}
|
2023-12-21 11:06:36 -04:00
|
|
|
if (rx_io_packet.count > sizeof(reg_direct_pwm.pwm)/2) {
|
2018-10-31 19:06:08 -03:00
|
|
|
return false;
|
|
|
|
}
|
2023-11-25 11:47:55 -04:00
|
|
|
/* copy channel data */
|
|
|
|
uint16_t i = 0, num_values = rx_io_packet.count;
|
2024-04-16 13:56:58 -03:00
|
|
|
while ((i < IOMCU_MAX_RC_CHANNELS) && (num_values > 0)) {
|
2018-09-14 07:06:59 -03:00
|
|
|
/* XXX range-check value? */
|
|
|
|
if (rx_io_packet.regs[i] != PWM_IGNORE_THIS_CHANNEL) {
|
2023-11-25 11:47:55 -04:00
|
|
|
reg_direct_pwm.pwm[i] = rx_io_packet.regs[i];
|
2018-09-14 07:06:59 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
num_values--;
|
|
|
|
i++;
|
|
|
|
}
|
2018-10-31 19:06:08 -03:00
|
|
|
fmu_data_received_time = last_ms;
|
2023-06-24 15:16:43 -03:00
|
|
|
chEvtSignalI(thread_ctx, IOEVENT_PWM);
|
2018-09-14 07:06:59 -03:00
|
|
|
break;
|
|
|
|
}
|
2018-10-30 23:10:51 -03:00
|
|
|
|
2023-11-25 11:47:55 -04:00
|
|
|
case PAGE_MIXING: { // multi-packet message
|
2018-10-31 19:06:08 -03:00
|
|
|
uint16_t offset = rx_io_packet.offset, num_values = rx_io_packet.count;
|
|
|
|
if (offset + num_values > sizeof(mixing)/2) {
|
|
|
|
return false;
|
|
|
|
}
|
2018-10-30 21:07:47 -03:00
|
|
|
memcpy(((uint16_t *)&mixing)+offset, &rx_io_packet.regs[0], num_values*2);
|
|
|
|
break;
|
|
|
|
}
|
2018-09-14 07:06:59 -03:00
|
|
|
|
2018-10-30 23:10:51 -03:00
|
|
|
case PAGE_FAILSAFE_PWM: {
|
2023-11-25 11:47:55 -04:00
|
|
|
if (rx_io_packet.count != sizeof(reg_failsafe_pwm.pwm)/2) {
|
2018-10-31 19:06:08 -03:00
|
|
|
return false;
|
|
|
|
}
|
2023-11-25 11:47:55 -04:00
|
|
|
memcpy((®_failsafe_pwm.pwm[0]), &rx_io_packet.regs[0], rx_io_packet.count*2);
|
2018-10-30 23:10:51 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2021-09-20 10:43:32 -03:00
|
|
|
case PAGE_GPIO:
|
|
|
|
if (rx_io_packet.count != 1) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
memcpy(&GPIO, &rx_io_packet.regs[0] + rx_io_packet.offset, sizeof(GPIO));
|
2023-05-24 17:43:17 -03:00
|
|
|
break;
|
|
|
|
|
|
|
|
case PAGE_DSHOT: {
|
2023-11-25 11:47:55 -04:00
|
|
|
if (rx_io_packet.count != sizeof(dshot)/2) {
|
2023-05-24 17:43:17 -03:00
|
|
|
return false;
|
|
|
|
}
|
2023-11-25 11:47:55 -04:00
|
|
|
memcpy(((uint16_t *)&dshot)+rx_io_packet.offset, &rx_io_packet.regs[0], rx_io_packet.count*2);
|
2023-05-24 17:43:17 -03:00
|
|
|
if(dshot.telem_mask) {
|
|
|
|
hal.rcout->set_telem_request_mask(dshot.telem_mask);
|
2021-09-20 10:43:32 -03:00
|
|
|
}
|
2023-05-24 17:43:17 -03:00
|
|
|
if (dshot.command) {
|
|
|
|
hal.rcout->send_dshot_command(dshot.command, dshot.chan, dshot.command_timeout_ms, dshot.repeat_count, dshot.priority);
|
|
|
|
}
|
|
|
|
|
2021-09-20 10:43:32 -03:00
|
|
|
break;
|
2023-05-24 17:43:17 -03:00
|
|
|
}
|
2021-09-20 10:43:32 -03:00
|
|
|
|
2018-09-14 07:06:59 -03:00
|
|
|
default:
|
|
|
|
break;
|
2018-05-02 08:22:17 -03:00
|
|
|
}
|
|
|
|
tx_io_packet.count = 0;
|
|
|
|
tx_io_packet.code = CODE_SUCCESS;
|
|
|
|
tx_io_packet.crc = 0;
|
2018-11-26 06:46:22 -04:00
|
|
|
tx_io_packet.page = 0;
|
|
|
|
tx_io_packet.offset = 0;
|
2018-05-02 08:22:17 -03:00
|
|
|
tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size());
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2018-05-05 15:16:52 -03:00
|
|
|
void AP_IOMCU_FW::schedule_reboot(uint32_t time_ms)
|
|
|
|
{
|
|
|
|
do_reboot = true;
|
2018-10-31 19:06:08 -03:00
|
|
|
reboot_time = last_ms + time_ms;
|
2018-05-05 15:16:52 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AP_IOMCU_FW::calculate_fw_crc(void)
|
|
|
|
{
|
|
|
|
#define APP_SIZE_MAX 0xf000
|
|
|
|
#define APP_LOAD_ADDRESS 0x08001000
|
2018-09-14 07:06:59 -03:00
|
|
|
// compute CRC of the current firmware
|
|
|
|
uint32_t sum = 0;
|
2018-05-05 15:16:52 -03:00
|
|
|
|
2018-09-14 07:06:59 -03:00
|
|
|
for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) {
|
|
|
|
uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS);
|
2019-10-27 17:35:54 -03:00
|
|
|
sum = crc32_small(sum, (const uint8_t *)&bytes, sizeof(bytes));
|
2018-09-14 07:06:59 -03:00
|
|
|
}
|
2018-05-05 15:16:52 -03:00
|
|
|
|
2018-09-14 07:06:59 -03:00
|
|
|
reg_setup.crc[0] = sum & 0xFFFF;
|
|
|
|
reg_setup.crc[1] = sum >> 16;
|
2018-05-05 15:16:52 -03:00
|
|
|
}
|
|
|
|
|
2018-05-10 21:37:01 -03:00
|
|
|
|
2018-05-10 22:43:28 -03:00
|
|
|
/*
|
|
|
|
update safety state
|
|
|
|
*/
|
|
|
|
void AP_IOMCU_FW::safety_update(void)
|
|
|
|
{
|
2018-10-31 19:06:08 -03:00
|
|
|
uint32_t now = last_ms;
|
2018-05-10 22:43:28 -03:00
|
|
|
if (now - safety_update_ms < 100) {
|
|
|
|
// update safety at 10Hz
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
safety_update_ms = now;
|
|
|
|
|
|
|
|
bool safety_pressed = palReadLine(HAL_GPIO_PIN_SAFETY_INPUT);
|
|
|
|
if (safety_pressed) {
|
|
|
|
if (reg_status.flag_safety_off && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_ON)) {
|
|
|
|
safety_pressed = false;
|
|
|
|
} else if ((!reg_status.flag_safety_off) && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_OFF)) {
|
|
|
|
safety_pressed = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (safety_pressed) {
|
|
|
|
safety_button_counter++;
|
|
|
|
} else {
|
|
|
|
safety_button_counter = 0;
|
|
|
|
}
|
|
|
|
if (safety_button_counter == 10) {
|
|
|
|
// safety has been pressed for 1 second, change state
|
|
|
|
reg_status.flag_safety_off = !reg_status.flag_safety_off;
|
2019-04-20 00:28:21 -03:00
|
|
|
if (reg_status.flag_safety_off) {
|
|
|
|
hal.rcout->force_safety_off();
|
|
|
|
} else {
|
|
|
|
hal.rcout->force_safety_on();
|
|
|
|
}
|
2018-05-10 22:43:28 -03:00
|
|
|
}
|
2024-08-13 09:59:08 -03:00
|
|
|
// update the armed state
|
|
|
|
hal.util->set_soft_armed((reg_setup.arming & P_SETUP_ARMING_FMU_ARMED) != 0);
|
2018-05-10 22:43:28 -03:00
|
|
|
|
2023-02-10 21:39:14 -04:00
|
|
|
#if IOMCU_ENABLE_RESET_TEST
|
|
|
|
{
|
2019-04-23 22:33:42 -03:00
|
|
|
// deliberate lockup of IOMCU on 5s button press, for testing
|
|
|
|
// watchdog
|
2023-02-10 21:39:14 -04:00
|
|
|
static uint32_t safety_test_counter;
|
|
|
|
static bool should_lockup;
|
|
|
|
if (palReadLine(HAL_GPIO_PIN_SAFETY_INPUT)) {
|
|
|
|
safety_test_counter++;
|
|
|
|
} else {
|
|
|
|
safety_test_counter = 0;
|
|
|
|
}
|
|
|
|
if (safety_test_counter == 50) {
|
|
|
|
should_lockup = true;
|
|
|
|
}
|
|
|
|
// wait for lockup for safety to be released so we don't end
|
|
|
|
// up in the bootloader
|
|
|
|
if (should_lockup && palReadLine(HAL_GPIO_PIN_SAFETY_INPUT) == 0) {
|
|
|
|
#if IOMCU_ENABLE_RESET_TEST == 1
|
|
|
|
// lockup with watchdog
|
|
|
|
while (true) {
|
|
|
|
hal.scheduler->delay(50);
|
|
|
|
palToggleLine(HAL_GPIO_PIN_SAFETY_LED);
|
2019-04-23 22:33:42 -03:00
|
|
|
}
|
2023-02-10 21:39:14 -04:00
|
|
|
#else
|
|
|
|
// hard fault to simulate power reset or software fault
|
|
|
|
void *foo = (void*)0xE000ED38;
|
|
|
|
typedef void (*fptr)();
|
|
|
|
fptr gptr = (fptr) (void *) foo;
|
|
|
|
gptr();
|
|
|
|
while (true) {}
|
|
|
|
#endif
|
2019-04-23 22:33:42 -03:00
|
|
|
}
|
|
|
|
}
|
2023-02-10 21:39:14 -04:00
|
|
|
#endif // IOMCU_ENABLE_RESET_TEST
|
2019-04-23 22:33:42 -03:00
|
|
|
|
2018-05-10 22:43:28 -03:00
|
|
|
led_counter = (led_counter+1) % 16;
|
|
|
|
const uint16_t led_pattern = reg_status.flag_safety_off?0xFFFF:0x5500;
|
|
|
|
palWriteLine(HAL_GPIO_PIN_SAFETY_LED, (led_pattern & (1U << led_counter))?0:1);
|
|
|
|
}
|
|
|
|
|
2018-05-11 03:31:04 -03:00
|
|
|
/*
|
|
|
|
update hal.rcout mode if needed
|
|
|
|
*/
|
2023-05-24 17:43:17 -03:00
|
|
|
void AP_IOMCU_FW::rcout_config_update(void)
|
2018-05-11 03:31:04 -03:00
|
|
|
{
|
2023-05-24 17:43:17 -03:00
|
|
|
// channels cannot be changed from within a lock zone
|
|
|
|
// so needs to be done here
|
|
|
|
if (GPIO.channel_mask != last_GPIO_channel_mask) {
|
|
|
|
for (uint8_t i=0; i<8; i++) {
|
|
|
|
if ((GPIO.channel_mask & (1U << i)) != 0) {
|
|
|
|
hal.rcout->disable_ch(i);
|
|
|
|
hal.gpio->pinMode(101+i, HAL_GPIO_OUTPUT);
|
|
|
|
} else {
|
|
|
|
hal.rcout->enable_ch(i);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
last_GPIO_channel_mask = GPIO.channel_mask;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (last_channel_mask != reg_setup.channel_mask) {
|
|
|
|
for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) {
|
|
|
|
if (reg_setup.channel_mask & 1U << i) {
|
|
|
|
hal.rcout->enable_ch(i);
|
|
|
|
} else {
|
|
|
|
hal.rcout->disable_ch(i);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
last_channel_mask = reg_setup.channel_mask;
|
2023-10-23 10:36:02 -03:00
|
|
|
// channel enablement will affect the reported output mode
|
|
|
|
uint32_t output_mask = 0;
|
|
|
|
reg_status.rcout_mode = hal.rcout->get_output_mode(output_mask);
|
|
|
|
reg_status.rcout_mask = uint8_t(0xFF & output_mask);
|
2023-05-24 17:43:17 -03:00
|
|
|
}
|
|
|
|
|
2023-06-24 15:16:43 -03:00
|
|
|
// see if there is anything to do, we only support setting the mode for a particular channel once
|
2023-06-23 17:15:14 -03:00
|
|
|
if ((last_output_mode_mask & mode_out.mask) == mode_out.mask
|
|
|
|
&& (last_output_bdmask & mode_out.bdmask) == mode_out.bdmask
|
2024-08-13 09:59:08 -03:00
|
|
|
&& (last_output_reversible_mask & mode_out.reversible_mask) == mode_out.reversible_mask
|
2023-06-23 17:15:14 -03:00
|
|
|
&& last_output_esc_type == mode_out.esc_type) {
|
2023-06-24 15:16:43 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
switch (mode_out.mode) {
|
|
|
|
case AP_HAL::RCOutput::MODE_PWM_DSHOT150:
|
|
|
|
case AP_HAL::RCOutput::MODE_PWM_DSHOT300:
|
2023-06-29 13:08:20 -03:00
|
|
|
#if defined(STM32F103xB) || defined(STM32F103x8)
|
|
|
|
case AP_HAL::RCOutput::MODE_PWM_DSHOT600:
|
2023-06-23 17:15:14 -03:00
|
|
|
#endif
|
|
|
|
#ifdef HAL_WITH_BIDIR_DSHOT
|
|
|
|
hal.rcout->set_bidir_dshot_mask(mode_out.bdmask);
|
2023-06-29 13:08:20 -03:00
|
|
|
#endif
|
2024-08-13 09:59:08 -03:00
|
|
|
hal.rcout->set_reversible_mask(mode_out.reversible_mask);
|
|
|
|
hal.rcout->set_dshot_esc_type(AP_HAL::RCOutput::DshotEscType(mode_out.esc_type));
|
2020-07-11 09:22:05 -03:00
|
|
|
hal.rcout->set_output_mode(mode_out.mask, (AP_HAL::RCOutput::output_mode)mode_out.mode);
|
2023-05-24 17:43:17 -03:00
|
|
|
// enabling dshot changes the memory allocation
|
|
|
|
reg_status.freemem = hal.util->available_memory();
|
2023-06-24 15:16:43 -03:00
|
|
|
last_output_mode_mask |= mode_out.mask;
|
2023-06-23 17:15:14 -03:00
|
|
|
last_output_bdmask |= mode_out.bdmask;
|
2024-08-13 09:59:08 -03:00
|
|
|
last_output_reversible_mask |= mode_out.reversible_mask;
|
2023-06-23 17:15:14 -03:00
|
|
|
last_output_esc_type = mode_out.esc_type;
|
2023-06-24 15:16:43 -03:00
|
|
|
break;
|
|
|
|
case AP_HAL::RCOutput::MODE_PWM_ONESHOT:
|
|
|
|
case AP_HAL::RCOutput::MODE_PWM_ONESHOT125:
|
2023-05-24 17:43:17 -03:00
|
|
|
// setup to use a 1Hz frequency, so we only get output when we trigger
|
|
|
|
hal.rcout->set_freq(mode_out.mask, 1);
|
|
|
|
hal.rcout->set_output_mode(mode_out.mask, (AP_HAL::RCOutput::output_mode)mode_out.mode);
|
2023-06-24 15:16:43 -03:00
|
|
|
last_output_mode_mask |= mode_out.mask;
|
|
|
|
break;
|
|
|
|
case AP_HAL::RCOutput::MODE_PWM_BRUSHED:
|
2020-07-11 09:22:05 -03:00
|
|
|
// default to 2kHz for all channels for brushed output
|
|
|
|
hal.rcout->set_freq(mode_out.mask, 2000);
|
2018-07-13 01:44:16 -03:00
|
|
|
hal.rcout->set_esc_scaling(1000, 2000);
|
2020-07-11 09:22:05 -03:00
|
|
|
hal.rcout->set_output_mode(mode_out.mask, AP_HAL::RCOutput::MODE_PWM_BRUSHED);
|
|
|
|
hal.rcout->set_freq(mode_out.mask, reg_setup.pwm_altrate);
|
2023-06-24 15:16:43 -03:00
|
|
|
last_output_mode_mask |= mode_out.mask;
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
break;
|
2018-07-13 01:44:16 -03:00
|
|
|
}
|
2023-06-17 19:26:56 -03:00
|
|
|
|
|
|
|
uint32_t output_mask = 0;
|
|
|
|
reg_status.rcout_mode = hal.rcout->get_output_mode(output_mask);
|
|
|
|
reg_status.rcout_mask = uint8_t(0xFF & output_mask);
|
2018-05-11 03:31:04 -03:00
|
|
|
}
|
|
|
|
|
2018-10-30 23:10:51 -03:00
|
|
|
/*
|
|
|
|
fill in failsafe PWM values
|
|
|
|
*/
|
|
|
|
void AP_IOMCU_FW::fill_failsafe_pwm(void)
|
|
|
|
{
|
2024-04-27 04:44:31 -03:00
|
|
|
for (uint8_t i=0; i<IOMCU_MAX_RC_CHANNELS; i++) {
|
2018-10-30 23:10:51 -03:00
|
|
|
if (reg_status.flag_safety_off) {
|
|
|
|
reg_direct_pwm.pwm[i] = reg_failsafe_pwm.pwm[i];
|
|
|
|
} else {
|
2021-08-29 09:17:40 -03:00
|
|
|
reg_direct_pwm.pwm[i] = 0;
|
2018-10-30 23:10:51 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
if (mixing.enabled) {
|
|
|
|
run_mixer();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-09-20 10:43:32 -03:00
|
|
|
void AP_IOMCU_FW::GPIO_write()
|
|
|
|
{
|
|
|
|
for (uint8_t i=0; i<8; i++) {
|
|
|
|
if ((GPIO.channel_mask & (1U << i)) != 0) {
|
|
|
|
hal.gpio->write(101+i, (GPIO.output_mask & (1U << i)) != 0);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-05-02 08:22:17 -03:00
|
|
|
AP_HAL_MAIN();
|