[Fw] Update REF protocols.

This commit is contained in:
peng-zhihui 2022-02-09 11:52:03 +08:00
parent f0e2c8da3a
commit fe1007c668
21 changed files with 3793 additions and 3705 deletions

View File

@ -1,2 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<?xml version="1.0" encoding="UTF-8"?>
<module classpath="CMake" type="CPP_MODULE" version="4" />

View File

@ -1,8 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/REF-STM32F4.iml" filepath="$PROJECT_DIR$/.idea/REF-STM32F4.iml" />
</modules>
</component>
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/REF-STM32F4.iml" filepath="$PROJECT_DIR$/.idea/REF-STM32F4.iml" />
</modules>
</component>
</project>

View File

@ -1,95 +1,89 @@
/* Includes ------------------------------------------------------------------*/
#include "communication.hpp"
#include "common_inc.h"
/* Private defines -----------------------------------------------------------*/
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
/* Private constant data -----------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
volatile bool endpointListValid = false;
/* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/
// @brief Sends a line on the specified output.
osThreadId_t commTaskHandle;
const osThreadAttr_t commTask_attributes = {
.name = "commTask",
.stack_size = 10000 * 4,
.priority = (osPriority_t) osPriorityNormal,
};
void InitCommunication(void)
{
printf("\r\nHello, REF v%.1f Started!\r\n", CONFIG_FW_VERSION);
// Start command handling thread
commTaskHandle = osThreadNew(CommunicationTask, NULL, &commTask_attributes);
while (!endpointListValid)
osDelay(1);
}
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
osThreadId_t usbIrqTaskHandle;
void UsbDeferredInterruptTask(void* ctx)
{
(void) ctx; // unused parameter
for (;;)
{
// Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
osStatus semaphore_status = osSemaphoreAcquire(sem_usb_irq, osWaitForever);
if (semaphore_status == osOK)
{
// We have a new incoming USB transmission: handle it
HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
// Let the irq (OTG_FS_IRQHandler) fire again.
HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
}
}
}
// Thread to handle deffered processing of USB interrupt, and
// read commands out of the UART DMA circular buffer
void CommunicationTask(void* ctx)
{
(void) ctx; // unused parameter
CommitProtocol();
// Allow main init to continue
endpointListValid = true;
StartUartServer();
StartUsbServer();
StartCanServer(CAN1);
StartCanServer(CAN2);
for (;;)
{
osDelay(1000); // nothing to do
}
}
extern "C" {
int _write(int file, const char* data, int len);
}
// @brief This is what printf calls internally
int _write(int file, const char* data, int len)
{
#ifdef DEBUG_VIA_USB_SERIAL
usbStreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
uart4StreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
#endif
uart5StreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
return len;
}
/* Includes ------------------------------------------------------------------*/
#include "communication.hpp"
#include "common_inc.h"
/* Private defines -----------------------------------------------------------*/
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
/* Private constant data -----------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
volatile bool endpointListValid = false;
/* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/
// @brief Sends a line on the specified output.
osThreadId_t commTaskHandle;
const osThreadAttr_t commTask_attributes = {
.name = "commTask",
.stack_size = 45000,
.priority = (osPriority_t) osPriorityNormal,
};
void InitCommunication(void)
{
// Start command handling thread
commTaskHandle = osThreadNew(CommunicationTask, nullptr, &commTask_attributes);
while (!endpointListValid)
osDelay(1);
}
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
osThreadId_t usbIrqTaskHandle;
void UsbDeferredInterruptTask(void* ctx)
{
(void) ctx; // unused parameter
for (;;)
{
// Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
osStatus semaphore_status = osSemaphoreAcquire(sem_usb_irq, osWaitForever);
if (semaphore_status == osOK)
{
// We have a new incoming USB transmission: handle it
HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
// Let the irq (OTG_FS_IRQHandler) fire again.
HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
}
}
}
// Thread to handle deffered processing of USB interrupt, and
// read commands out of the UART DMA circular buffer
void CommunicationTask(void* ctx)
{
(void) ctx; // unused parameter
CommitProtocol();
// Allow main init to continue
endpointListValid = true;
StartUartServer();
StartUsbServer();
StartCanServer(CAN1);
StartCanServer(CAN2);
for (;;)
{
osDelay(1000); // nothing to do
}
}
extern "C" {
int _write(int file, const char* data, int len);
}
// @brief This is what printf calls internally
int _write(int file, const char* data, int len)
{
usbStreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
uart4StreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
return len;
}

View File

@ -1,252 +1,250 @@
/*
*
* Zero-config node ID negotiation
* -------------------------------
*
* A heartbeat message is a message with a 8 byte unique serial number as payload.
* A regular message is any message that is not a heartbeat message.
*
* All nodes MUST obey these four rules:
*
* a) At a given point in time, a node MUST consider a node ID taken (by others)
* if any of the following is true:
* - the node received a (not self-emitted) heartbeat message with that node ID
* within the last second
* - the node attempted and failed at sending a heartbeat message with that
* node ID within the last second (failed in the sense of not ACK'd)
*
* b) At a given point in time, a node MUST NOT consider a node ID self-assigned
* if, within the last second, it did not succeed in sending a heartbeat
* message with that node ID.
*
* c) At a given point in time, a node MUST NOT send any heartbeat message with
* a node ID that is taken.
*
* d) At a given point in time, a node MUST NOT send any regular message with
* a node ID that is not self-assigned.
*
* Hardware allocation
* -------------------
* RX FIFO0:
* - filter bank 0: heartbeat messages
*/
#include "common_inc.h"
#include <stm32f4xx_hal.h>
#include <cmsis_os.h>
// defined in can.c
extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
CAN_context can1Ctx;
CAN_context can2Ctx;
static CAN_context* ctxs = nullptr;
static CAN_RxHeaderTypeDef headerRx;
static uint8_t data[8];
struct CAN_context* get_can_ctx(CAN_HandleTypeDef* hcan)
{
if (hcan->Instance == CAN1)
return &can1Ctx;
else if (hcan->Instance == CAN2)
return &can2Ctx;
else
return nullptr;
}
bool StartCanServer(CAN_TypeDef* hcan)
{
if (hcan == CAN1)
{
ctxs = &can1Ctx;
ctxs->handle = &hcan1;
} else if (hcan == CAN2)
{
ctxs = &can2Ctx;
ctxs->handle = &hcan2;
} else
return false; // fail if none of the above checks matched
HAL_StatusTypeDef status;
ctxs->node_id = 0;
ctxs->serial_number = serialNumber;
osSemaphoreDef(sem_send_heartbeat);
ctxs->sem_send_heartbeat = osSemaphoreNew(1, 0, osSemaphore(sem_send_heartbeat));
//// Set up filter
CAN_FilterTypeDef sFilterConfig = {
.FilterIdHigh = 0x0000,
.FilterIdLow = 0x0000,
.FilterMaskIdHigh = 0x0000,
.FilterMaskIdLow = 0x0000,
.FilterFIFOAssignment = CAN_RX_FIFO0,
.FilterBank = 0,
.FilterMode = CAN_FILTERMODE_IDMASK,
.FilterScale = CAN_FILTERSCALE_16BIT, // two 16-bit filters
.FilterActivation = ENABLE,
.SlaveStartFilterBank = 0
};
status = HAL_CAN_ConfigFilter(ctxs->handle, &sFilterConfig);
if (status != HAL_OK)
return false;
status = HAL_CAN_Start(ctxs->handle);
if (status != HAL_OK)
return false;
status = HAL_CAN_ActivateNotification(ctxs->handle,
CAN_IT_TX_MAILBOX_EMPTY |
CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING |
/* we probably only want this */
CAN_IT_RX_FIFO0_FULL | CAN_IT_RX_FIFO1_FULL |
CAN_IT_RX_FIFO0_OVERRUN | CAN_IT_RX_FIFO1_OVERRUN |
CAN_IT_WAKEUP | CAN_IT_SLEEP_ACK |
CAN_IT_ERROR_WARNING | CAN_IT_ERROR_PASSIVE |
CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE |
CAN_IT_ERROR);
if (status != HAL_OK)
return false;
return true;
}
void tx_complete_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx)
{
// CAN_context* ctx = get_can_ctx(hcan);
// if (!ctx) return;
// ctx->tx_msg_cnt++;
if (hcan->Instance == CAN1)
osSemaphoreRelease(sem_can1_tx);
else if (hcan->Instance == CAN2)
osSemaphoreRelease(sem_can2_tx);
}
void tx_aborted_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx)
{
if (!get_can_ctx(hcan))
return;
get_can_ctx(hcan)->TxMailboxAbortCallbackCnt++;
}
void tx_error(CAN_context* ctx, uint8_t mailbox_idx)
{
}
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef* hcan)
{ tx_complete_callback(hcan, 0); }
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef* hcan)
{ tx_complete_callback(hcan, 1); }
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef* hcan)
{ tx_complete_callback(hcan, 2); }
void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef* hcan)
{ tx_aborted_callback(hcan, 0); }
void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef* hcan)
{ tx_aborted_callback(hcan, 1); }
void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef* hcan)
{ tx_aborted_callback(hcan, 2); }
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan)
{
CAN_context* ctx = get_can_ctx(hcan);
if (!ctx) return;
ctx->received_msg_cnt++;
HAL_StatusTypeDef status = HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &headerRx, data);
if (status != HAL_OK)
{
ctx->unexpected_errors++;
return;
}
OnCanMessage(ctx, &headerRx, data);
}
void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo0FullCallbackCnt++; }
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1MsgPendingCallbackCnt++; }
void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1FullCallbackCnt++; }
void HAL_CAN_SleepCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->SleepCallbackCnt++; }
void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->WakeUpFromRxMsgCallbackCnt++; }
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef* hcan)
{
//__asm volatile ("bkpt");
CAN_context* ctx = get_can_ctx(hcan);
if (!ctx) return;
volatile uint32_t original_error = hcan->ErrorCode;
(void) original_error;
// handle transmit errors in all three mailboxes
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST0)
{
SET_BIT(hcan->Instance->sTxMailBox[0].TIR, CAN_TI0R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST0;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR0)
{
tx_error(ctx, 0);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR0;
}
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST1)
{
SET_BIT(hcan->Instance->sTxMailBox[1].TIR, CAN_TI1R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST1;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR1)
{
tx_error(ctx, 1);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR1;
}
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST2)
{
SET_BIT(hcan->Instance->sTxMailBox[2].TIR, CAN_TI2R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST2;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR2)
{
tx_error(ctx, 2);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR2;
}
if (hcan->ErrorCode)
ctx->unexpected_errors++;
}
void CanSendMessage(CAN_context* canCtx, uint8_t* txData, CAN_TxHeaderTypeDef* txHeader)
{
osStatus semaphore_status;
if (canCtx->handle->Instance == CAN1)
semaphore_status = osSemaphoreAcquire(sem_can1_tx, osWaitForever);
else if (canCtx->handle->Instance == CAN2)
semaphore_status = osSemaphoreAcquire(sem_can2_tx, osWaitForever);
else
return;
if (semaphore_status == osOK)
{
HAL_CAN_AddTxMessage(canCtx->handle, txHeader, txData, &canCtx->last_heartbeat_mailbox);
}
}
/*
*
* Zero-config node ID negotiation
* -------------------------------
*
* A heartbeat message is a message with a 8 byte unique serial number as payload.
* A regular message is any message that is not a heartbeat message.
*
* All nodes MUST obey these four rules:
*
* a) At a given point in time, a node MUST consider a node ID taken (by others)
* if any of the following is true:
* - the node received a (not self-emitted) heartbeat message with that node ID
* within the last second
* - the node attempted and failed at sending a heartbeat message with that
* node ID within the last second (failed in the sense of not ACK'd)
*
* b) At a given point in time, a node MUST NOT consider a node ID self-assigned
* if, within the last second, it did not succeed in sending a heartbeat
* message with that node ID.
*
* c) At a given point in time, a node MUST NOT send any heartbeat message with
* a node ID that is taken.
*
* d) At a given point in time, a node MUST NOT send any regular message with
* a node ID that is not self-assigned.
*
* Hardware allocation
* -------------------
* RX FIFO0:
* - filter bank 0: heartbeat messages
*/
#include "common_inc.h"
#include <stm32f4xx_hal.h>
#include <cmsis_os.h>
// defined in can.c
extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
CAN_context can1Ctx;
CAN_context can2Ctx;
static CAN_context* ctxs = nullptr;
static CAN_RxHeaderTypeDef headerRx;
static uint8_t data[8];
struct CAN_context* get_can_ctx(CAN_HandleTypeDef* hcan)
{
if (hcan->Instance == CAN1)
return &can1Ctx;
else if (hcan->Instance == CAN2)
return &can2Ctx;
else
return nullptr;
}
bool StartCanServer(CAN_TypeDef* hcan)
{
if (hcan == CAN1)
{
ctxs = &can1Ctx;
ctxs->handle = &hcan1;
} else if (hcan == CAN2)
{
ctxs = &can2Ctx;
ctxs->handle = &hcan2;
} else
return false; // fail if none of the above checks matched
HAL_StatusTypeDef status;
ctxs->node_id = 0;
ctxs->serial_number = serialNumber;
osSemaphoreDef(sem_send_heartbeat);
ctxs->sem_send_heartbeat = osSemaphoreNew(1, 0, osSemaphore(sem_send_heartbeat));
//// Set up filter
CAN_FilterTypeDef sFilterConfig = {
.FilterIdHigh = 0x0000,
.FilterIdLow = 0x0000,
.FilterMaskIdHigh = 0x0000,
.FilterMaskIdLow = 0x0000,
.FilterFIFOAssignment = CAN_RX_FIFO0,
.FilterBank = 0,
.FilterMode = CAN_FILTERMODE_IDMASK,
.FilterScale = CAN_FILTERSCALE_16BIT, // two 16-bit filters
.FilterActivation = ENABLE,
.SlaveStartFilterBank = 0
};
status = HAL_CAN_ConfigFilter(ctxs->handle, &sFilterConfig);
if (status != HAL_OK)
return false;
status = HAL_CAN_Start(ctxs->handle);
if (status != HAL_OK)
return false;
status = HAL_CAN_ActivateNotification(ctxs->handle,
CAN_IT_TX_MAILBOX_EMPTY |
CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING |
/* we probably only want this */
CAN_IT_RX_FIFO0_FULL | CAN_IT_RX_FIFO1_FULL |
CAN_IT_RX_FIFO0_OVERRUN | CAN_IT_RX_FIFO1_OVERRUN |
CAN_IT_WAKEUP | CAN_IT_SLEEP_ACK |
CAN_IT_ERROR_WARNING | CAN_IT_ERROR_PASSIVE |
CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE |
CAN_IT_ERROR);
if (status != HAL_OK)
return false;
return true;
}
void tx_complete_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx)
{
// CAN_context* ctx = get_can_ctx(hcan);
// if (!ctx) return;
// ctx->tx_msg_cnt++;
if (hcan->Instance == CAN1)
osSemaphoreRelease(sem_can1_tx);
else if (hcan->Instance == CAN2)
osSemaphoreRelease(sem_can2_tx);
}
void tx_aborted_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx)
{
if (!get_can_ctx(hcan))
return;
get_can_ctx(hcan)->TxMailboxAbortCallbackCnt++;
}
void tx_error(CAN_context* ctx, uint8_t mailbox_idx)
{
}
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef* hcan)
{ tx_complete_callback(hcan, 0); }
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef* hcan)
{ tx_complete_callback(hcan, 1); }
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef* hcan)
{ tx_complete_callback(hcan, 2); }
void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef* hcan)
{ tx_aborted_callback(hcan, 0); }
void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef* hcan)
{ tx_aborted_callback(hcan, 1); }
void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef* hcan)
{ tx_aborted_callback(hcan, 2); }
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan)
{
CAN_context* ctx = get_can_ctx(hcan);
if (!ctx) return;
ctx->received_msg_cnt++;
HAL_StatusTypeDef status = HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &headerRx, data);
if (status != HAL_OK)
{
ctx->unexpected_errors++;
return;
}
OnCanMessage(ctx, &headerRx, data);
}
void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo0FullCallbackCnt++; }
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1MsgPendingCallbackCnt++; }
void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1FullCallbackCnt++; }
void HAL_CAN_SleepCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->SleepCallbackCnt++; }
void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef* hcan)
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->WakeUpFromRxMsgCallbackCnt++; }
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef* hcan)
{
//__asm volatile ("bkpt");
CAN_context* ctx = get_can_ctx(hcan);
if (!ctx) return;
volatile uint32_t original_error = hcan->ErrorCode;
(void) original_error;
// handle transmit errors in all three mailboxes
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST0)
{
SET_BIT(hcan->Instance->sTxMailBox[0].TIR, CAN_TI0R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST0;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR0)
{
tx_error(ctx, 0);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR0;
}
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST1)
{
SET_BIT(hcan->Instance->sTxMailBox[1].TIR, CAN_TI1R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST1;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR1)
{
tx_error(ctx, 1);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR1;
}
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST2)
{
SET_BIT(hcan->Instance->sTxMailBox[2].TIR, CAN_TI2R_TXRQ);
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST2;
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR2)
{
tx_error(ctx, 2);
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR2;
}
if (hcan->ErrorCode)
ctx->unexpected_errors++;
}
void CanSendMessage(CAN_context* canCtx, uint8_t* txData, CAN_TxHeaderTypeDef* txHeader)
{
osStatus semaphore_status;
if (canCtx->handle->Instance == CAN1)
semaphore_status = osSemaphoreAcquire(sem_can1_tx, osWaitForever);
else if (canCtx->handle->Instance == CAN2)
semaphore_status = osSemaphoreAcquire(sem_can2_tx, osWaitForever);
else
return;
if (semaphore_status == osOK)
HAL_CAN_AddTxMessage(canCtx->handle, txHeader, txData, &canCtx->last_heartbeat_mailbox);
}

View File

@ -1,206 +1,206 @@
#include "common_inc.h"
#include "interface_uart.hpp"
#include "ascii_processor.hpp"
#include "fibre/protocol.hpp"
#include "usart.h"
#define UART_TX_BUFFER_SIZE 64
#define UART_RX_BUFFER_SIZE 64
// DMA open loop continous circular buffer
// 1ms delay periodic, chase DMA ptr around
static uint8_t dma_rx_buffer[2][UART_RX_BUFFER_SIZE];
static uint32_t dma_last_rcv_idx[2];
// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable
// static thread_local uint32_t deadline_ms = 0;
osThreadId_t uartServerTaskHandle;
class UART4Sender : public StreamSink
{
public:
UART4Sender()
{
channelType = CHANNEL_TYPE_UART4;
}
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override
{
// Loop to ensure all bytes get sent
while (length)
{
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
// wait for USB interface to become ready
// TODO: implement ring buffer to get a more continuous stream of data
// if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
if (osSemaphoreAcquire(sem_uart4_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
return -1;
// transmit chunk
memcpy(tx_buf_, buffer, chunk);
if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK)
return -1;
buffer += chunk;
length -= chunk;
if (processed_bytes)
*processed_bytes += chunk;
}
return 0;
}
size_t get_free_space() override
{ return SIZE_MAX; }
private:
uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
} uart4_stream_output;
class UART5Sender : public StreamSink
{
public:
UART5Sender()
{
channelType = CHANNEL_TYPE_UART5;
}
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override
{
// Loop to ensure all bytes get sent
while (length)
{
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
// wait for USB interface to become ready
// TODO: implement ring buffer to get a more continuous stream of data
// if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
if (osSemaphoreAcquire(sem_uart5_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
return -1;
// transmit chunk
memcpy(tx_buf_, buffer, chunk);
if (HAL_UART_Transmit_DMA(&huart5, tx_buf_, chunk) != HAL_OK)
return -1;
buffer += chunk;
length -= chunk;
if (processed_bytes)
*processed_bytes += chunk;
}
return 0;
}
size_t get_free_space() override
{ return SIZE_MAX; }
private:
uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
} uart5_stream_output;
StreamSink* uart4StreamOutputPtr = &uart4_stream_output;
StreamBasedPacketSink uart4_packet_output(uart4_stream_output);
BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output);
StreamToPacketSegmenter uart4_stream_input(uart4_channel);
StreamSink* uart5StreamOutputPtr = &uart5_stream_output;
StreamBasedPacketSink uart5_packet_output(uart5_stream_output);
BidirectionalPacketBasedChannel uart5_channel(uart5_packet_output);
StreamToPacketSegmenter uart5_stream_input(uart5_channel);
static void UartServerTask(void* ctx)
{
(void) ctx;
for (;;)
{
// Check for UART errors and restart recieve DMA transfer if required
if (huart4.ErrorCode != HAL_UART_ERROR_NONE)
{
HAL_UART_AbortReceive(&huart4);
HAL_UART_Receive_DMA(&huart4, dma_rx_buffer[0], sizeof(dma_rx_buffer[0]));
}
// Fetch the circular buffer "write pointer", where it would write next
uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
// deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < dma_last_rcv_idx[0])
{
uart4_stream_input.process_bytes(dma_rx_buffer[0] + dma_last_rcv_idx[0],
UART_RX_BUFFER_SIZE - dma_last_rcv_idx[0],
nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0],
UART_RX_BUFFER_SIZE - dma_last_rcv_idx[0], uart4_stream_output);
dma_last_rcv_idx[0] = 0;
}
if (new_rcv_idx > dma_last_rcv_idx[0])
{
uart4_stream_input.process_bytes(dma_rx_buffer[0] + dma_last_rcv_idx[0],
new_rcv_idx - dma_last_rcv_idx[0],
nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0],
new_rcv_idx - dma_last_rcv_idx[0], uart4_stream_output);
dma_last_rcv_idx[0] = new_rcv_idx;
}
// Check for UART errors and restart recieve DMA transfer if required
if (huart5.ErrorCode != HAL_UART_ERROR_NONE)
{
HAL_UART_AbortReceive(&huart5);
HAL_UART_Receive_DMA(&huart5, dma_rx_buffer[1], sizeof(dma_rx_buffer[1]));
}
// Fetch the circular buffer "write pointer", where it would write next
new_rcv_idx = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR;
// deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < dma_last_rcv_idx[1])
{
uart4_stream_input.process_bytes(dma_rx_buffer[1] + dma_last_rcv_idx[1],
UART_RX_BUFFER_SIZE - dma_last_rcv_idx[1],
nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1],
UART_RX_BUFFER_SIZE - dma_last_rcv_idx[1], uart5_stream_output);
dma_last_rcv_idx[1] = 0;
}
if (new_rcv_idx > dma_last_rcv_idx[1])
{
uart4_stream_input.process_bytes(dma_rx_buffer[1] + dma_last_rcv_idx[1],
new_rcv_idx - dma_last_rcv_idx[1],
nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1],
new_rcv_idx - dma_last_rcv_idx[1], uart5_stream_output);
dma_last_rcv_idx[1] = new_rcv_idx;
}
osDelay(1);
};
}
const osThreadAttr_t uartServerTask_attributes = {
.name = "UartServerTask",
.stack_size = 1000 * 4,
.priority = (osPriority_t) osPriorityNormal,
};
void StartUartServer()
{
// DMA is set up to recieve in a circular buffer forever.
// We dont use interrupts to fetch the data, instead we periodically read
// data out of the circular buffer into a parse buffer, controlled by a state machine
HAL_UART_Receive_DMA(&huart4, dma_rx_buffer[0], sizeof(dma_rx_buffer[0]));
dma_last_rcv_idx[0] = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
HAL_UART_Receive_DMA(&huart5, dma_rx_buffer[1], sizeof(dma_rx_buffer[1]));
dma_last_rcv_idx[1] = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR;
// Start UART communication thread
uartServerTaskHandle = osThreadNew(UartServerTask, nullptr, &uartServerTask_attributes);
}
void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart)
{
if (huart->Instance == UART4)
osSemaphoreRelease(sem_uart4_dma);
else if (huart->Instance == UART5)
osSemaphoreRelease(sem_uart5_dma);
}
#include "common_inc.h"
#include "interface_uart.hpp"
#include "ascii_processor.hpp"
#include "fibre/protocol.hpp"
#include "usart.h"
#define UART_TX_BUFFER_SIZE 64
#define UART_RX_BUFFER_SIZE 64
// DMA open loop continous circular buffer
// 1ms delay periodic, chase DMA ptr around
static uint8_t dma_rx_buffer[2][UART_RX_BUFFER_SIZE];
static uint32_t dma_last_rcv_idx[2];
// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable
// static thread_local uint32_t deadline_ms = 0;
osThreadId_t uartServerTaskHandle;
class UART4Sender : public StreamSink
{
public:
UART4Sender()
{
channelType = CHANNEL_TYPE_UART4;
}
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override
{
// Loop to ensure all bytes get sent
while (length)
{
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
// wait for USB interface to become ready
// TODO: implement ring buffer to get a more continuous stream of data
// if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
if (osSemaphoreAcquire(sem_uart4_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
return -1;
// transmit chunk
memcpy(tx_buf_, buffer, chunk);
if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK)
return -1;
buffer += chunk;
length -= chunk;
if (processed_bytes)
*processed_bytes += chunk;
}
return 0;
}
size_t get_free_space() override
{ return SIZE_MAX; }
private:
uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
} uart4_stream_output;
class UART5Sender : public StreamSink
{
public:
UART5Sender()
{
channelType = CHANNEL_TYPE_UART5;
}
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override
{
// Loop to ensure all bytes get sent
while (length)
{
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
// wait for USB interface to become ready
// TODO: implement ring buffer to get a more continuous stream of data
// if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
if (osSemaphoreAcquire(sem_uart5_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
return -1;
// transmit chunk
memcpy(tx_buf_, buffer, chunk);
if (HAL_UART_Transmit_DMA(&huart5, tx_buf_, chunk) != HAL_OK)
return -1;
buffer += chunk;
length -= chunk;
if (processed_bytes)
*processed_bytes += chunk;
}
return 0;
}
size_t get_free_space() override
{ return SIZE_MAX; }
private:
uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
} uart5_stream_output;
StreamSink* uart4StreamOutputPtr = &uart4_stream_output;
StreamBasedPacketSink uart4_packet_output(uart4_stream_output);
BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output);
StreamToPacketSegmenter uart4_stream_input(uart4_channel);
StreamSink* uart5StreamOutputPtr = &uart5_stream_output;
StreamBasedPacketSink uart5_packet_output(uart5_stream_output);
BidirectionalPacketBasedChannel uart5_channel(uart5_packet_output);
StreamToPacketSegmenter uart5_stream_input(uart5_channel);
static void UartServerTask(void* ctx)
{
(void) ctx;
for (;;)
{
// Check for UART errors and restart recieve DMA transfer if required
if (huart4.ErrorCode != HAL_UART_ERROR_NONE)
{
HAL_UART_AbortReceive(&huart4);
HAL_UART_Receive_DMA(&huart4, dma_rx_buffer[0], sizeof(dma_rx_buffer[0]));
}
// Fetch the circular buffer "write pointer", where it would write next
uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
// deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < dma_last_rcv_idx[0])
{
uart4_stream_input.process_bytes(dma_rx_buffer[0] + dma_last_rcv_idx[0],
UART_RX_BUFFER_SIZE - dma_last_rcv_idx[0],
nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0],
UART_RX_BUFFER_SIZE - dma_last_rcv_idx[0], uart4_stream_output);
dma_last_rcv_idx[0] = 0;
}
if (new_rcv_idx > dma_last_rcv_idx[0])
{
uart4_stream_input.process_bytes(dma_rx_buffer[0] + dma_last_rcv_idx[0],
new_rcv_idx - dma_last_rcv_idx[0],
nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0],
new_rcv_idx - dma_last_rcv_idx[0], uart4_stream_output);
dma_last_rcv_idx[0] = new_rcv_idx;
}
// Check for UART errors and restart recieve DMA transfer if required
if (huart5.ErrorCode != HAL_UART_ERROR_NONE)
{
HAL_UART_AbortReceive(&huart5);
HAL_UART_Receive_DMA(&huart5, dma_rx_buffer[1], sizeof(dma_rx_buffer[1]));
}
// Fetch the circular buffer "write pointer", where it would write next
new_rcv_idx = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR;
// deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < dma_last_rcv_idx[1])
{
uart4_stream_input.process_bytes(dma_rx_buffer[1] + dma_last_rcv_idx[1],
UART_RX_BUFFER_SIZE - dma_last_rcv_idx[1],
nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1],
UART_RX_BUFFER_SIZE - dma_last_rcv_idx[1], uart5_stream_output);
dma_last_rcv_idx[1] = 0;
}
if (new_rcv_idx > dma_last_rcv_idx[1])
{
uart4_stream_input.process_bytes(dma_rx_buffer[1] + dma_last_rcv_idx[1],
new_rcv_idx - dma_last_rcv_idx[1],
nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1],
new_rcv_idx - dma_last_rcv_idx[1], uart5_stream_output);
dma_last_rcv_idx[1] = new_rcv_idx;
}
osDelay(1);
};
}
const osThreadAttr_t uartServerTask_attributes = {
.name = "UartServerTask",
.stack_size = 2000,
.priority = (osPriority_t) osPriorityNormal,
};
void StartUartServer()
{
// DMA is set up to receive in a circular buffer forever.
// We don't use interrupts to fetch the data, instead we periodically read
// data out of the circular buffer into a parse buffer, controlled by a state machine
HAL_UART_Receive_DMA(&huart4, dma_rx_buffer[0], sizeof(dma_rx_buffer[0]));
dma_last_rcv_idx[0] = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
HAL_UART_Receive_DMA(&huart5, dma_rx_buffer[1], sizeof(dma_rx_buffer[1]));
dma_last_rcv_idx[1] = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR;
// Start UART communication thread
uartServerTaskHandle = osThreadNew(UartServerTask, nullptr, &uartServerTask_attributes);
}
void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart)
{
if (huart->Instance == UART4)
osSemaphoreRelease(sem_uart4_dma);
else if (huart->Instance == UART5)
osSemaphoreRelease(sem_uart5_dma);
}

View File

@ -1,190 +1,190 @@
#include "common_inc.h"
#include "ascii_processor.hpp"
#include "usbd_cdc.h"
#include "usbd_cdc_if.h"
#include "usb_device.h"
#include "interface_usb.hpp"
osThreadId_t usbServerTaskHandle;
USBStats_t usb_stats_ = {0};
class USBSender : public PacketSink
{
public:
USBSender(uint8_t endpoint_pair, const osSemaphoreId &sem_usb_tx)
: endpoint_pair_(endpoint_pair), sem_usb_tx_(sem_usb_tx)
{}
int process_packet(const uint8_t *buffer, size_t length) override
{
// cannot send partial packets
if (length > USB_TX_DATA_SIZE)
return -1;
// wait for USB interface to become ready
if (osSemaphoreAcquire(sem_usb_tx_, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
{
// If the host resets the device it might be that the TX-complete handler is never called
// and the sem_usb_tx_ semaphore is never released. To handle this we just override the
// TX buffer if this wait times out. The implication is that the channel is no longer lossless.
// TODO: handle endpoint reset properly
usb_stats_.tx_overrun_cnt++;
}
// transmit packet
uint8_t status = CDC_Transmit_FS(const_cast<uint8_t *>(buffer), length, endpoint_pair_);
if (status != USBD_OK)
{
osSemaphoreRelease(sem_usb_tx_);
return -1;
}
usb_stats_.tx_cnt++;
return 0;
}
private:
uint8_t endpoint_pair_;
const osSemaphoreId &sem_usb_tx_;
};
// Note we could have independent semaphores here to allow concurrent transmission
USBSender usb_packet_output_cdc(CDC_OUT_EP, sem_usb_tx);
USBSender usb_packet_output_native(ODRIVE_OUT_EP, sem_usb_tx);
class TreatPacketSinkAsStreamSink : public StreamSink
{
public:
TreatPacketSinkAsStreamSink(PacketSink &output) : output_(output)
{
channelType = CHANNEL_TYPE_USB;
}
int process_bytes(const uint8_t *buffer, size_t length, size_t *processed_bytes)
{
// Loop to ensure all bytes get sent
while (length)
{
size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE;
if (output_.process_packet(buffer, length) != 0)
return -1;
buffer += chunk;
length -= chunk;
if (processed_bytes)
*processed_bytes += chunk;
}
return 0;
}
size_t get_free_space()
{ return SIZE_MAX; }
private:
PacketSink &output_;
} usb_stream_output(usb_packet_output_cdc);
// This is used by the printf feature. Hence the above statics, and below seemingly random ptr (it's externed)
// TODO: less spaghetti code
StreamSink *usbStreamOutputPtr = &usb_stream_output;
BidirectionalPacketBasedChannel usb_channel(usb_packet_output_native);
struct USBInterface
{
uint8_t *rx_buf = nullptr;
uint32_t rx_len = 0;
bool data_pending = false;
uint8_t out_ep;
uint8_t in_ep;
USBSender &usb_sender;
};
// Note: statics make this less modular.
// Note: we use a single rx semaphore and loop over data_pending to allow a single pump loop thread
static USBInterface CDC_interface = {
.rx_buf = nullptr,
.rx_len = 0,
.data_pending = false,
.out_ep = CDC_OUT_EP,
.in_ep = CDC_IN_EP,
.usb_sender = usb_packet_output_cdc,
};
static USBInterface ODrive_interface = {
.rx_buf = nullptr,
.rx_len = 0,
.data_pending = false,
.out_ep = ODRIVE_OUT_EP,
.in_ep = ODRIVE_IN_EP,
.usb_sender = usb_packet_output_native,
};
static void UsbServerTask(void *ctx)
{
(void) ctx;
for (;;)
{
// const uint32_t usb_check_timeout = 1; // ms
osStatus sem_stat = osSemaphoreAcquire(sem_usb_rx, osWaitForever);
if (sem_stat == osOK)
{
usb_stats_.rx_cnt++;
// CDC Interface
if (CDC_interface.data_pending)
{
CDC_interface.data_pending = false;
ASCII_protocol_parse_stream(CDC_interface.rx_buf, CDC_interface.rx_len, usb_stream_output);
USBD_CDC_ReceivePacket(&hUsbDeviceFS, CDC_interface.out_ep); // Allow next packet
}
// Native Interface
if (ODrive_interface.data_pending)
{
ODrive_interface.data_pending = false;
usb_channel.process_packet(ODrive_interface.rx_buf, ODrive_interface.rx_len);
USBD_CDC_ReceivePacket(&hUsbDeviceFS, ODrive_interface.out_ep); // Allow next packet
}
}
}
}
// Called from CDC_Receive_FS callback function, this allows the communication
// thread to handle the incoming data
void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair)
{
USBInterface *usb_iface;
if (endpoint_pair == CDC_interface.out_ep)
{
usb_iface = &CDC_interface;
} else if (endpoint_pair == ODrive_interface.out_ep)
{
usb_iface = &ODrive_interface;
} else
{
return;
}
// We don't allow the next USB packet until the previous one has been processed completely.
// Therefore it's safe to write to these vars directly since we know previous processing is complete.
usb_iface->rx_buf = buf;
usb_iface->rx_len = len;
usb_iface->data_pending = true;
osSemaphoreRelease(sem_usb_rx);
}
const osThreadAttr_t usbServerTask_attributes = {
.name = "UsbServerTask",
.stack_size = 512 * 4,
.priority = (osPriority_t) osPriorityNormal,
};
void StartUsbServer()
{
// Start USB communication thread
usbServerTaskHandle = osThreadNew(UsbServerTask, nullptr, &usbServerTask_attributes);
}
#include "common_inc.h"
#include "ascii_processor.hpp"
#include "usbd_cdc.h"
#include "usbd_cdc_if.h"
#include "usb_device.h"
#include "interface_usb.hpp"
osThreadId_t usbServerTaskHandle;
USBStats_t usb_stats_ = {0};
class USBSender : public PacketSink
{
public:
USBSender(uint8_t endpoint_pair, const osSemaphoreId &sem_usb_tx)
: endpoint_pair_(endpoint_pair), sem_usb_tx_(sem_usb_tx)
{}
int process_packet(const uint8_t *buffer, size_t length) override
{
// cannot send partial packets
if (length > USB_TX_DATA_SIZE)
return -1;
// wait for USB interface to become ready
if (osSemaphoreAcquire(sem_usb_tx_, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
{
// If the host resets the device it might be that the TX-complete handler is never called
// and the sem_usb_tx_ semaphore is never released. To handle this we just override the
// TX buffer if this wait times out. The implication is that the channel is no longer lossless.
// TODO: handle endpoint reset properly
usb_stats_.tx_overrun_cnt++;
}
// transmit packet
uint8_t status = CDC_Transmit_FS(const_cast<uint8_t *>(buffer), length, endpoint_pair_);
if (status != USBD_OK)
{
osSemaphoreRelease(sem_usb_tx_);
return -1;
}
usb_stats_.tx_cnt++;
return 0;
}
private:
uint8_t endpoint_pair_;
const osSemaphoreId &sem_usb_tx_;
};
// Note we could have independent semaphores here to allow concurrent transmission
USBSender usb_packet_output_cdc(CDC_OUT_EP, sem_usb_tx);
USBSender usb_packet_output_native(ODRIVE_OUT_EP, sem_usb_tx);
class TreatPacketSinkAsStreamSink : public StreamSink
{
public:
TreatPacketSinkAsStreamSink(PacketSink &output) : output_(output)
{
channelType = CHANNEL_TYPE_USB;
}
int process_bytes(const uint8_t *buffer, size_t length, size_t *processed_bytes)
{
// Loop to ensure all bytes get sent
while (length)
{
size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE;
if (output_.process_packet(buffer, length) != 0)
return -1;
buffer += chunk;
length -= chunk;
if (processed_bytes)
*processed_bytes += chunk;
}
return 0;
}
size_t get_free_space()
{ return SIZE_MAX; }
private:
PacketSink &output_;
} usb_stream_output(usb_packet_output_cdc);
// This is used by the printf feature. Hence the above statics, and below seemingly random ptr (it's externed)
// TODO: less spaghetti code
StreamSink *usbStreamOutputPtr = &usb_stream_output;
BidirectionalPacketBasedChannel usb_channel(usb_packet_output_native);
struct USBInterface
{
uint8_t *rx_buf = nullptr;
uint32_t rx_len = 0;
bool data_pending = false;
uint8_t out_ep;
uint8_t in_ep;
USBSender &usb_sender;
};
// Note: statics make this less modular.
// Note: we use a single rx semaphore and loop over data_pending to allow a single pump loop thread
static USBInterface CDC_interface = {
.rx_buf = nullptr,
.rx_len = 0,
.data_pending = false,
.out_ep = CDC_OUT_EP,
.in_ep = CDC_IN_EP,
.usb_sender = usb_packet_output_cdc,
};
static USBInterface ODrive_interface = {
.rx_buf = nullptr,
.rx_len = 0,
.data_pending = false,
.out_ep = ODRIVE_OUT_EP,
.in_ep = ODRIVE_IN_EP,
.usb_sender = usb_packet_output_native,
};
static void UsbServerTask(void *ctx)
{
(void) ctx;
for (;;)
{
// const uint32_t usb_check_timeout = 1; // ms
osStatus sem_stat = osSemaphoreAcquire(sem_usb_rx, osWaitForever);
if (sem_stat == osOK)
{
usb_stats_.rx_cnt++;
// CDC Interface
if (CDC_interface.data_pending)
{
CDC_interface.data_pending = false;
ASCII_protocol_parse_stream(CDC_interface.rx_buf, CDC_interface.rx_len, usb_stream_output);
USBD_CDC_ReceivePacket(&hUsbDeviceFS, CDC_interface.out_ep); // Allow next packet
}
// Native Interface
if (ODrive_interface.data_pending)
{
ODrive_interface.data_pending = false;
usb_channel.process_packet(ODrive_interface.rx_buf, ODrive_interface.rx_len);
USBD_CDC_ReceivePacket(&hUsbDeviceFS, ODrive_interface.out_ep); // Allow next packet
}
}
}
}
// Called from CDC_Receive_FS callback function, this allows the communication
// thread to handle the incoming data
void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair)
{
USBInterface *usb_iface;
if (endpoint_pair == CDC_interface.out_ep)
{
usb_iface = &CDC_interface;
} else if (endpoint_pair == ODrive_interface.out_ep)
{
usb_iface = &ODrive_interface;
} else
{
return;
}
// We don't allow the next USB packet until the previous one has been processed completely.
// Therefore it's safe to write to these vars directly since we know previous processing is complete.
usb_iface->rx_buf = buf;
usb_iface->rx_len = len;
usb_iface->data_pending = true;
osSemaphoreRelease(sem_usb_rx);
}
const osThreadAttr_t usbServerTask_attributes = {
.name = "UsbServerTask",
.stack_size = 2000,
.priority = (osPriority_t) osPriorityNormal,
};
void StartUsbServer()
{
// Start USB communication thread
usbServerTaskHandle = osThreadNew(UsbServerTask, nullptr, &usbServerTask_attributes);
}

View File

@ -1,114 +1,113 @@
#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE!
set(CMAKE_SYSTEM_NAME Generic)
set(CMAKE_SYSTEM_VERSION 1)
cmake_minimum_required(VERSION 3.19)
# specify cross compilers and tools
set(CMAKE_C_COMPILER arm-none-eabi-gcc)
set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
set(CMAKE_ASM_COMPILER arm-none-eabi-gcc)
set(CMAKE_AR arm-none-eabi-ar)
set(CMAKE_OBJCOPY arm-none-eabi-objcopy)
set(CMAKE_OBJDUMP arm-none-eabi-objdump)
set(SIZE arm-none-eabi-size)
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
# project settings
project(Core-STM32F4-fw C CXX ASM)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_C_STANDARD 11)
# for use printf & scanf with float
set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float")
#Uncomment for hardware floating point
add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING)
add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
#Uncomment for software floating point
#add_compile_options(-mfloat-abi=soft)
add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0)
# uncomment to mitigate c++17 absolute addresses warnings
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register")
if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
message(STATUS "Maximum optimization for speed")
add_compile_options(-Ofast)
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
message(STATUS "Maximum optimization for speed, debug info included")
add_compile_options(-Ofast -g)
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
message(STATUS "Maximum optimization for size")
add_compile_options(-Os)
else ()
message(STATUS "Minimal optimization, debug info included")
add_compile_options(-Og -g)
endif ()
include_directories(
Core/Inc
Drivers/STM32F4xx_HAL_Driver/Inc
Drivers/STM32F4xx_HAL_Driver/Inc/Legacy
Drivers/CMSIS/Device/ST/STM32F4xx/Include
Drivers/CMSIS/Include
Middlewares/Third_Party/FreeRTOS/Source/include
Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
Middlewares/ST/STM32_USB_Device_Library/Core/Inc
Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc
USB_DEVICE/App
USB_DEVICE/Target
3rdParty/fibre/cpp/include
3rdParty/u8g2
3rdParty/u8g2/cpp
Bsp
Bsp/imu
Bsp/imu/filters
Bsp/communication
Bsp/memory
Bsp/utils
Bsp/gpio
Bsp/utils/software_i2c
Bsp/utils/arm_math
Robot
UserApp
)
add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP)
add_definitions(-DDEBUG_VIA_USB_SERIAL)
file(GLOB_RECURSE SOURCES
"startup/*.*"
"Drivers/*.*"
"Core/*.*"
"UserApp/*.*"
"3rdParty/*.*"
"Middlewares/*.*"
"USB_DEVICE/*.*"
"Robot/*.*"
"Bsp/*.*"
)
set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32F405RGTx_FLASH.ld)
add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=${PROJECT_BINARY_DIR}/${PROJECT_NAME}.map)
add_link_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
add_link_options(-T ${LINKER_SCRIPT})
link_directories("Drivers/CMSIS/Lib")
link_libraries("arm_cortexM4lf_math.a")
add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex)
set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin)
add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD
COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:${PROJECT_NAME}.elf> ${HEX_FILE}
COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${PROJECT_NAME}.elf> ${BIN_FILE}
COMMENT "Building ${HEX_FILE}
Building ${BIN_FILE}")
#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE!
set(CMAKE_SYSTEM_NAME Generic)
set(CMAKE_SYSTEM_VERSION 1)
cmake_minimum_required(VERSION 3.19)
# specify cross compilers and tools
set(CMAKE_C_COMPILER arm-none-eabi-gcc)
set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
set(CMAKE_ASM_COMPILER arm-none-eabi-gcc)
set(CMAKE_AR arm-none-eabi-ar)
set(CMAKE_OBJCOPY arm-none-eabi-objcopy)
set(CMAKE_OBJDUMP arm-none-eabi-objdump)
set(SIZE arm-none-eabi-size)
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
# project settings
project(Core-STM32F4-fw C CXX ASM)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_C_STANDARD 11)
# for use printf & scanf with float
set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float")
#Uncomment for hardware floating point
add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING)
add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
#Uncomment for software floating point
#add_compile_options(-mfloat-abi=soft)
add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0)
# uncomment to mitigate c++17 absolute addresses warnings
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register")
if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
message(STATUS "Maximum optimization for speed")
add_compile_options(-Ofast)
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
message(STATUS "Maximum optimization for speed, debug info included")
add_compile_options(-Ofast -g)
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
message(STATUS "Maximum optimization for size")
add_compile_options(-Os)
else ()
message(STATUS "Minimal optimization, debug info included")
add_compile_options(-Og -g)
endif ()
include_directories(
Core/Inc
Drivers/STM32F4xx_HAL_Driver/Inc
Drivers/STM32F4xx_HAL_Driver/Inc/Legacy
Drivers/CMSIS/Device/ST/STM32F4xx/Include
Drivers/CMSIS/Include
Middlewares/Third_Party/FreeRTOS/Source/include
Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
Middlewares/ST/STM32_USB_Device_Library/Core/Inc
Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc
USB_DEVICE/App
USB_DEVICE/Target
3rdParty/fibre/cpp/include
3rdParty/u8g2
3rdParty/u8g2/cpp
Bsp
Bsp/imu
Bsp/imu/filters
Bsp/communication
Bsp/memory
Bsp/utils
Bsp/gpio
Bsp/utils/software_i2c
Bsp/utils/arm_math
Robot
UserApp
)
add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP)
file(GLOB_RECURSE SOURCES
"startup/*.*"
"Drivers/*.*"
"Core/*.*"
"UserApp/*.*"
"3rdParty/*.*"
"Middlewares/*.*"
"USB_DEVICE/*.*"
"Robot/*.*"
"Bsp/*.*"
)
set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32F405RGTx_FLASH.ld)
add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=${PROJECT_BINARY_DIR}/${PROJECT_NAME}.map)
add_link_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
add_link_options(-T ${LINKER_SCRIPT})
link_directories("Drivers/CMSIS/Lib")
link_libraries("arm_cortexM4lf_math.a")
add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex)
set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin)
add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD
COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:${PROJECT_NAME}.elf> ${HEX_FILE}
COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${PROJECT_NAME}.elf> ${BIN_FILE}
COMMENT "Building ${HEX_FILE}
Building ${BIN_FILE}")

View File

@ -1,114 +1,113 @@
#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE!
set(CMAKE_SYSTEM_NAME Generic)
set(CMAKE_SYSTEM_VERSION 1)
cmake_minimum_required(VERSION 3.19)
# specify cross compilers and tools
set(CMAKE_C_COMPILER arm-none-eabi-gcc)
set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
set(CMAKE_ASM_COMPILER arm-none-eabi-gcc)
set(CMAKE_AR arm-none-eabi-ar)
set(CMAKE_OBJCOPY arm-none-eabi-objcopy)
set(CMAKE_OBJDUMP arm-none-eabi-objdump)
set(SIZE arm-none-eabi-size)
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
# project settings
project(Core-STM32F4-fw C CXX ASM)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_C_STANDARD 11)
# for use printf & scanf with float
set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float")
#Uncomment for hardware floating point
add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING)
add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
#Uncomment for software floating point
#add_compile_options(-mfloat-abi=soft)
add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0)
# uncomment to mitigate c++17 absolute addresses warnings
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register")
if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
message(STATUS "Maximum optimization for speed")
add_compile_options(-Ofast)
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
message(STATUS "Maximum optimization for speed, debug info included")
add_compile_options(-Ofast -g)
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
message(STATUS "Maximum optimization for size")
add_compile_options(-Os)
else ()
message(STATUS "Minimal optimization, debug info included")
add_compile_options(-Og -g)
endif ()
include_directories(
Core/Inc
Drivers/STM32F4xx_HAL_Driver/Inc
Drivers/STM32F4xx_HAL_Driver/Inc/Legacy
Drivers/CMSIS/Device/ST/STM32F4xx/Include
Drivers/CMSIS/Include
Middlewares/Third_Party/FreeRTOS/Source/include
Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
Middlewares/ST/STM32_USB_Device_Library/Core/Inc
Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc
USB_DEVICE/App
USB_DEVICE/Target
3rdParty/fibre/cpp/include
3rdParty/u8g2
3rdParty/u8g2/cpp
Bsp
Bsp/imu
Bsp/imu/filters
Bsp/communication
Bsp/memory
Bsp/utils
Bsp/gpio
Bsp/utils/software_i2c
Bsp/utils/arm_math
Robot
UserApp
)
add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP)
# add_definitions(-DDEBUG_VIA_USB_SERIAL)
file(GLOB_RECURSE SOURCES
"startup/*.*"
"Drivers/*.*"
"Core/*.*"
"UserApp/*.*"
"3rdParty/*.*"
"Middlewares/*.*"
"USB_DEVICE/*.*"
"Robot/*.*"
"Bsp/*.*"
)
set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32F405RGTx_FLASH.ld)
add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=${PROJECT_BINARY_DIR}/${PROJECT_NAME}.map)
add_link_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
add_link_options(-T ${LINKER_SCRIPT})
link_directories("Drivers/CMSIS/Lib")
link_libraries("arm_cortexM4lf_math.a")
add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex)
set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin)
add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD
COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:${PROJECT_NAME}.elf> ${HEX_FILE}
COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${PROJECT_NAME}.elf> ${BIN_FILE}
COMMENT "Building ${HEX_FILE}
Building ${BIN_FILE}")
#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE!
set(CMAKE_SYSTEM_NAME Generic)
set(CMAKE_SYSTEM_VERSION 1)
cmake_minimum_required(VERSION 3.19)
# specify cross compilers and tools
set(CMAKE_C_COMPILER arm-none-eabi-gcc)
set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
set(CMAKE_ASM_COMPILER arm-none-eabi-gcc)
set(CMAKE_AR arm-none-eabi-ar)
set(CMAKE_OBJCOPY arm-none-eabi-objcopy)
set(CMAKE_OBJDUMP arm-none-eabi-objdump)
set(SIZE arm-none-eabi-size)
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
# project settings
project(Core-STM32F4-fw C CXX ASM)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_C_STANDARD 11)
# for use printf & scanf with float
set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float")
#Uncomment for hardware floating point
add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING)
add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
#Uncomment for software floating point
#add_compile_options(-mfloat-abi=soft)
add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0)
# uncomment to mitigate c++17 absolute addresses warnings
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register")
if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
message(STATUS "Maximum optimization for speed")
add_compile_options(-Ofast)
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
message(STATUS "Maximum optimization for speed, debug info included")
add_compile_options(-Ofast -g)
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
message(STATUS "Maximum optimization for size")
add_compile_options(-Os)
else ()
message(STATUS "Minimal optimization, debug info included")
add_compile_options(-Og -g)
endif ()
include_directories(
Core/Inc
Drivers/STM32F4xx_HAL_Driver/Inc
Drivers/STM32F4xx_HAL_Driver/Inc/Legacy
Drivers/CMSIS/Device/ST/STM32F4xx/Include
Drivers/CMSIS/Include
Middlewares/Third_Party/FreeRTOS/Source/include
Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
Middlewares/ST/STM32_USB_Device_Library/Core/Inc
Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc
USB_DEVICE/App
USB_DEVICE/Target
3rdParty/fibre/cpp/include
3rdParty/u8g2
3rdParty/u8g2/cpp
Bsp
Bsp/imu
Bsp/imu/filters
Bsp/communication
Bsp/memory
Bsp/utils
Bsp/gpio
Bsp/utils/software_i2c
Bsp/utils/arm_math
Robot
UserApp
)
add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP)
file(GLOB_RECURSE SOURCES
"startup/*.*"
"Drivers/*.*"
"Core/*.*"
"UserApp/*.*"
"3rdParty/*.*"
"Middlewares/*.*"
"USB_DEVICE/*.*"
"Robot/*.*"
"Bsp/*.*"
)
set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32F405RGTx_FLASH.ld)
add_link_options(-Wl,-gc-sections,--print-memory-usage,-Map=${PROJECT_BINARY_DIR}/${PROJECT_NAME}.map)
add_link_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
add_link_options(-T ${LINKER_SCRIPT})
link_directories("Drivers/CMSIS/Lib")
link_libraries("arm_cortexM4lf_math.a")
add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex)
set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin)
add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD
COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:${PROJECT_NAME}.elf> ${HEX_FILE}
COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${PROJECT_NAME}.elf> ${BIN_FILE}
COMMENT "Building ${HEX_FILE}
Building ${BIN_FILE}")

View File

@ -1,175 +1,175 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* File Name : freertos.c
* Description : Code for freertos applications
******************************************************************************
* @attention
*
* <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
* All rights reserved.</center></h2>
*
* This software component is licensed by ST under Ultimate Liberty license
* SLA0044, the "License"; You may not use this file except in compliance with
* the License. You may obtain a copy of the License at:
* www.st.com/SLA0044
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "common_inc.h"
#include "communication.hpp"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
// List of semaphores
osSemaphoreId sem_usb_irq;
osSemaphoreId sem_uart4_dma;
osSemaphoreId sem_uart5_dma;
osSemaphoreId sem_usb_rx;
osSemaphoreId sem_usb_tx;
osSemaphoreId sem_can1_tx;
osSemaphoreId sem_can2_tx;
/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
.name = "defaultTask",
.stack_size = 500 * 4,
.priority = (osPriority_t) osPriorityNormal,
};
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
/* USER CODE END FunctionPrototypes */
void StartDefaultTask(void *argument);
extern void MX_USB_DEVICE_Init(void);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
/**
* @brief FreeRTOS initialization
* @param None
* @retval None
*/
void MX_FREERTOS_Init(void) {
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* USER CODE BEGIN RTOS_MUTEX */
/* add mutexes, ... */
/* USER CODE END RTOS_MUTEX */
/* USER CODE BEGIN RTOS_SEMAPHORES */
// Init usb irq binary semaphore, and start with no tokens by removing the starting one.
osSemaphoreDef(sem_usb_irq);
sem_usb_irq = osSemaphoreNew(1, 0, osSemaphore(sem_usb_irq));
// Create a semaphore for UART DMA and remove a token
osSemaphoreDef(sem_uart4_dma);
sem_uart4_dma = osSemaphoreNew(1, 1, osSemaphore(sem_uart4_dma));
osSemaphoreDef(sem_uart5_dma);
sem_uart5_dma = osSemaphoreNew(1, 1, osSemaphore(sem_uart5_dma));
// Create a semaphore for USB RX, and start with no tokens by removing the starting one.
osSemaphoreDef(sem_usb_rx);
sem_usb_rx = osSemaphoreNew(1, 0, osSemaphore(sem_usb_rx));
// Create a semaphore for USB TX
osSemaphoreDef(sem_usb_tx);
sem_usb_tx = osSemaphoreNew(1, 1, osSemaphore(sem_usb_tx));
// Create a semaphore for CAN TX
osSemaphoreDef(sem_can1_tx);
sem_can1_tx = osSemaphoreNew(1, 1, osSemaphore(sem_can1_tx));
osSemaphoreDef(sem_can2_tx);
sem_can2_tx = osSemaphoreNew(1, 1, osSemaphore(sem_can2_tx));
/* USER CODE END RTOS_SEMAPHORES */
/* USER CODE BEGIN RTOS_TIMERS */
/* USER CODE END RTOS_TIMERS */
/* USER CODE BEGIN RTOS_QUEUES */
// This Task must run before MX_USB_DEVICE_Init(), so have to put it here.
const osThreadAttr_t usbIrqTask_attributes = {
.name = "usbIrqTask",
.stack_size = 128 * 4,
.priority = (osPriority_t) osPriorityAboveNormal,
};
usbIrqTaskHandle = osThreadNew(UsbDeferredInterruptTask, NULL, &usbIrqTask_attributes);
/* USER CODE END RTOS_QUEUES */
/* Create the thread(s) */
/* creation of defaultTask */
defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);
/* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */
/* USER CODE END RTOS_THREADS */
/* USER CODE BEGIN RTOS_EVENTS */
/* add events, ... */
/* USER CODE END RTOS_EVENTS */
}
/* USER CODE BEGIN Header_StartDefaultTask */
/**
* @brief Function implementing the defaultTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
/* init code for USB_DEVICE */
MX_USB_DEVICE_Init();
/* USER CODE BEGIN StartDefaultTask */
// Invoke cpp-version main().
Main();
vTaskDelete(defaultTaskHandle);
/* USER CODE END StartDefaultTask */
}
/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
/* USER CODE END Application */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/* USER CODE BEGIN Header */
/**
******************************************************************************
* File Name : freertos.c
* Description : Code for freertos applications
******************************************************************************
* @attention
*
* <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
* All rights reserved.</center></h2>
*
* This software component is licensed by ST under Ultimate Liberty license
* SLA0044, the "License"; You may not use this file except in compliance with
* the License. You may obtain a copy of the License at:
* www.st.com/SLA0044
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "common_inc.h"
#include "communication.hpp"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
// List of semaphores
osSemaphoreId sem_usb_irq;
osSemaphoreId sem_uart4_dma;
osSemaphoreId sem_uart5_dma;
osSemaphoreId sem_usb_rx;
osSemaphoreId sem_usb_tx;
osSemaphoreId sem_can1_tx;
osSemaphoreId sem_can2_tx;
/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
.name = "defaultTask",
.stack_size = 2000,
.priority = (osPriority_t) osPriorityNormal,
};
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
/* USER CODE END FunctionPrototypes */
void StartDefaultTask(void *argument);
extern void MX_USB_DEVICE_Init(void);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
/**
* @brief FreeRTOS initialization
* @param None
* @retval None
*/
void MX_FREERTOS_Init(void) {
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* USER CODE BEGIN RTOS_MUTEX */
/* add mutexes, ... */
/* USER CODE END RTOS_MUTEX */
/* USER CODE BEGIN RTOS_SEMAPHORES */
// Init usb irq binary semaphore, and start with no tokens by removing the starting one.
osSemaphoreDef(sem_usb_irq);
sem_usb_irq = osSemaphoreNew(1, 0, osSemaphore(sem_usb_irq));
// Create a semaphore for UART DMA and remove a token
osSemaphoreDef(sem_uart4_dma);
sem_uart4_dma = osSemaphoreNew(1, 1, osSemaphore(sem_uart4_dma));
osSemaphoreDef(sem_uart5_dma);
sem_uart5_dma = osSemaphoreNew(1, 1, osSemaphore(sem_uart5_dma));
// Create a semaphore for USB RX, and start with no tokens by removing the starting one.
osSemaphoreDef(sem_usb_rx);
sem_usb_rx = osSemaphoreNew(1, 0, osSemaphore(sem_usb_rx));
// Create a semaphore for USB TX
osSemaphoreDef(sem_usb_tx);
sem_usb_tx = osSemaphoreNew(1, 1, osSemaphore(sem_usb_tx));
// Create a semaphore for CAN TX
osSemaphoreDef(sem_can1_tx);
sem_can1_tx = osSemaphoreNew(1, 1, osSemaphore(sem_can1_tx));
osSemaphoreDef(sem_can2_tx);
sem_can2_tx = osSemaphoreNew(1, 1, osSemaphore(sem_can2_tx));
/* USER CODE END RTOS_SEMAPHORES */
/* USER CODE BEGIN RTOS_TIMERS */
/* USER CODE END RTOS_TIMERS */
/* USER CODE BEGIN RTOS_QUEUES */
// This Task must run before MX_USB_DEVICE_Init(), so have to put it here.
const osThreadAttr_t usbIrqTask_attributes = {
.name = "usbIrqTask",
.stack_size = 500,
.priority = (osPriority_t) osPriorityAboveNormal,
};
usbIrqTaskHandle = osThreadNew(UsbDeferredInterruptTask, NULL, &usbIrqTask_attributes);
/* USER CODE END RTOS_QUEUES */
/* Create the thread(s) */
/* creation of defaultTask */
defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);
/* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */
/* USER CODE END RTOS_THREADS */
/* USER CODE BEGIN RTOS_EVENTS */
/* add events, ... */
/* USER CODE END RTOS_EVENTS */
}
/* USER CODE BEGIN Header_StartDefaultTask */
/**
* @brief Function implementing the defaultTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
/* init code for USB_DEVICE */
MX_USB_DEVICE_Init();
/* USER CODE BEGIN StartDefaultTask */
// Invoke cpp-version main().
Main();
vTaskDelete(defaultTaskHandle);
/* USER CODE END StartDefaultTask */
}
/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
/* USER CODE END Application */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

File diff suppressed because it is too large Load Diff

View File

@ -1,348 +1,317 @@
#include "ctrl_step.hpp"
#include "communication.hpp"
CtrlStepMotor::CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse,
uint8_t _reduction, float _minAngle, float _maxAngle) :
nodeID(_id), hcan(_hcan), inverseDirection(_inverse), reduction(_reduction),
angleLimitMin(_minAngle), angleLimitMax(_maxAngle)
{
txHeader =
{
.StdId = 0,
.ExtId = 0,
.IDE = CAN_ID_STD,
.RTR = CAN_RTR_DATA,
.DLC = 8,
.TransmitGlobalTime = DISABLE
};
}
void CtrlStepMotor::SetEnable(bool _enable)
{
state = _enable ? FINISH : STOP;
uint8_t mode = 0x01;
txHeader.StdId = nodeID << 7 | mode;
// Int to Bytes
uint32_t val = _enable ? 1 : 0;
auto* b = (unsigned char*) &val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::DoCalibration()
{
uint8_t mode = 0x02;
txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetCurrentSetPoint(float _val)
{
state = RUNNING;
uint8_t mode = 0x03;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetVelocitySetPoint(float _val)
{
state = RUNNING;
uint8_t mode = 0x04;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetPositionSetPoint(float _val)
{
state = RUNNING;
uint8_t mode = 0x05;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetPositionWithTime(float _pos, float _time)
{
state = RUNNING;
uint8_t mode = 0x06;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_pos;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
b = (unsigned char*) &_time;
for (int i = 4; i < 8; i++)
canBuf[i] = *(b + i - 4);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::AddTrajectoryPoint(float _pos, float _vel)
{
uint8_t mode = 0x07;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_pos;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
b = (unsigned char*) &_vel;
for (int i = 4; i < 8; i++)
canBuf[i] = *(b + i - 4);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetNodeID(uint32_t _id)
{
uint8_t mode = 0x11;
txHeader.StdId = nodeID << 7 | mode;
// Int to Bytes
auto* b = (unsigned char*) &_id;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetCurrentLimit(float _val)
{
uint8_t mode = 0x12;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetVelocityLimit(float _val)
{
uint8_t mode = 0x13;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetAcceleration(float _val, bool _storeToMem)
{
uint8_t mode = 0x14;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = _storeToMem ? 1 : 0;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::ApplyPositionAsHome()
{
uint8_t mode = 0x15;
txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetEnableOnBoot(bool _enable)
{
uint8_t mode = 0x16;
txHeader.StdId = nodeID << 7 | mode;
// Int to Bytes
uint32_t val = _enable ? 1 : 0;
auto* b = (unsigned char*) &val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetEnableAck(bool _enable)
{
uint8_t mode = 0x1B;
txHeader.StdId = nodeID << 7 | mode;
uint32_t val = _enable ? 1 : 0;
auto* b = (unsigned char*) &val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetEnableStallProtect(bool _enable)
{
uint8_t mode = 0x1C;
txHeader.StdId = nodeID << 7 | mode;
uint32_t val = _enable ? 1 : 0;
auto* b = (unsigned char*) &val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::Reboot()
{
uint8_t mode = 0x7f;
txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::EraseConfigs()
{
uint8_t mode = 0x7e;
txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetAngle(float _angle)
{
_angle = inverseDirection ? -_angle : _angle;
if (_angle <= angleLimitMax && _angle >= angleLimitMin)
{
float stepMotorCnt = _angle / 360.0f * reduction * CTRL_CIRCLE_COUNT;
SetPositionSetPoint(stepMotorCnt);
}
}
void CtrlStepMotor::SetAngleWithTime(float _angle, float _time)
{
_angle = inverseDirection ? -_angle : _angle;
float stepMotorCnt = _angle / 360.0f * reduction;
SetPositionWithTime(stepMotorCnt, _time);
}
void CtrlStepMotor::UpdateAngle()
{
uint8_t mode = 0x23;
txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::UpdateAngleCallback(float _pos, bool _isAck)
{
if (_isAck)
state = FINISH;
float tmp = _pos / (float) reduction * 360;
angle = inverseDirection ? -tmp : tmp;
}
void CtrlStepMotor::SetDceKp(int32_t _val)
{
uint8_t mode = 0x17;
txHeader.StdId = nodeID << 7 | mode;
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetDceKv(int32_t _val)
{
uint8_t mode = 0x18;
txHeader.StdId = nodeID << 7 | mode;
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetDceKi(int32_t _val)
{
uint8_t mode = 0x19;
txHeader.StdId = nodeID << 7 | mode;
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetDceKd(int32_t _val)
{
uint8_t mode = 0x1A;
txHeader.StdId = nodeID << 7 | mode;
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
#include "ctrl_step.hpp"
#include "communication.hpp"
CtrlStepMotor::CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse,
uint8_t _reduction, float _angleLimitMin, float _angleLimitMax) :
nodeID(_id), hcan(_hcan), inverseDirection(_inverse), reduction(_reduction),
angleLimitMin(_angleLimitMin), angleLimitMax(_angleLimitMax)
{
txHeader =
{
.StdId = 0,
.ExtId = 0,
.IDE = CAN_ID_STD,
.RTR = CAN_RTR_DATA,
.DLC = 8,
.TransmitGlobalTime = DISABLE
};
}
void CtrlStepMotor::SetEnable(bool _enable)
{
state = _enable ? FINISH : STOP;
uint8_t mode = 0x01;
txHeader.StdId = nodeID << 7 | mode;
// Int to Bytes
uint32_t val = _enable ? 1 : 0;
auto* b = (unsigned char*) &val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::DoCalibration()
{
uint8_t mode = 0x02;
txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetCurrentSetPoint(float _val)
{
state = RUNNING;
uint8_t mode = 0x03;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetVelocitySetPoint(float _val)
{
state = RUNNING;
uint8_t mode = 0x04;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetPositionSetPoint(float _val)
{
uint8_t mode = 0x05;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need ACK
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetPositionWithVelocityLimit(float _pos, float _vel)
{
uint8_t mode = 0x07;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_pos;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
b = (unsigned char*) &_vel;
for (int i = 4; i < 8; i++)
canBuf[i] = *(b + i - 4);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetNodeID(uint32_t _id)
{
uint8_t mode = 0x11;
txHeader.StdId = nodeID << 7 | mode;
// Int to Bytes
auto* b = (unsigned char*) &_id;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetCurrentLimit(float _val)
{
uint8_t mode = 0x12;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetVelocityLimit(float _val)
{
uint8_t mode = 0x13;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetAcceleration(float _val)
{
uint8_t mode = 0x14;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 0; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::ApplyPositionAsHome()
{
uint8_t mode = 0x15;
txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetEnableOnBoot(bool _enable)
{
uint8_t mode = 0x16;
txHeader.StdId = nodeID << 7 | mode;
// Int to Bytes
uint32_t val = _enable ? 1 : 0;
auto* b = (unsigned char*) &val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetEnableStallProtect(bool _enable)
{
uint8_t mode = 0x1B;
txHeader.StdId = nodeID << 7 | mode;
uint32_t val = _enable ? 1 : 0;
auto* b = (unsigned char*) &val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::Reboot()
{
uint8_t mode = 0x7f;
txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::EraseConfigs()
{
uint8_t mode = 0x7e;
txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetAngle(float _angle)
{
_angle = inverseDirection ? -_angle : _angle;
float stepMotorCnt = _angle / 360.0f * (float) reduction;
SetPositionSetPoint(stepMotorCnt);
}
void CtrlStepMotor::SetAngleWithVelocityLimit(float _angle, float _vel)
{
_angle = inverseDirection ? -_angle : _angle;
float stepMotorCnt = _angle / 360.0f * (float) reduction;
SetPositionWithVelocityLimit(stepMotorCnt, _vel);
}
void CtrlStepMotor::UpdateAngle()
{
uint8_t mode = 0x23;
txHeader.StdId = nodeID << 7 | mode;
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::UpdateAngleCallback(float _pos, bool _isFinished)
{
state = _isFinished ? FINISH : RUNNING;
float tmp = _pos / (float) reduction * 360;
angle = inverseDirection ? -tmp : tmp;
}
void CtrlStepMotor::SetDceKp(int32_t _val)
{
uint8_t mode = 0x17;
txHeader.StdId = nodeID << 7 | mode;
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetDceKv(int32_t _val)
{
uint8_t mode = 0x18;
txHeader.StdId = nodeID << 7 | mode;
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetDceKi(int32_t _val)
{
uint8_t mode = 0x19;
txHeader.StdId = nodeID << 7 | mode;
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetDceKd(int32_t _val)
{
uint8_t mode = 0x1A;
txHeader.StdId = nodeID << 7 | mode;
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}

View File

@ -1,99 +1,95 @@
#ifndef DUMMY_CORE_FW_CTRL_STEP_HPP
#define DUMMY_CORE_FW_CTRL_STEP_HPP
#include "fibre/protocol.hpp"
#include "can.h"
class CtrlStepMotor
{
public:
enum State
{
RUNNING,
FINISH,
STOP
};
const uint32_t CTRL_CIRCLE_COUNT = 200 * 256;
CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse = false, uint8_t _reduction = 1,
float _minAngle = -180, float _maxAngle = 180);
uint8_t nodeID;
float angle = 0;
float angleLimitMax;
float angleLimitMin;
bool inverseDirection;
uint8_t reduction;
State state = STOP;
void SetAngle(float _angle);
void SetAngleWithTime(float _angle, float _time);
// CAN Command
void SetEnable(bool _enable);
void DoCalibration();
void SetCurrentSetPoint(float _val);
void SetVelocitySetPoint(float _val);
void SetPositionSetPoint(float _val);
void SetPositionWithTime(float _pos, float _time);
void AddTrajectoryPoint(float _pos, float _vel);
void SetNodeID(uint32_t _id);
void SetCurrentLimit(float _val);
void SetVelocityLimit(float _val);
void SetAcceleration(float _val, bool _storeToMem = false);
void SetDceKp(int32_t _val);
void SetDceKv(int32_t _val);
void SetDceKi(int32_t _val);
void SetDceKd(int32_t _val);
void ApplyPositionAsHome();
void SetEnableOnBoot(bool _enable);
void SetEnableAck(bool _enable);
void SetEnableStallProtect(bool _enable);
void Reboot();
void EraseConfigs();
void UpdateAngle();
void UpdateAngleCallback(float _pos, bool _isAck);
// Communication protocol definitions
auto MakeProtocolDefinitions()
{
return make_protocol_member_list(
make_protocol_ro_property("angle", &angle),
make_protocol_function("reboot", *this, &CtrlStepMotor::Reboot),
make_protocol_function("erase_configs", *this, &CtrlStepMotor::EraseConfigs),
make_protocol_function("set_enable", *this, &CtrlStepMotor::SetEnable, "enable"),
make_protocol_function("set_position_with_time", *this, &CtrlStepMotor::SetPositionWithTime,
"pos", "time"),
make_protocol_function("set_position", *this, &CtrlStepMotor::SetPositionSetPoint, "pos"),
make_protocol_function("set_velocity", *this, &CtrlStepMotor::SetVelocitySetPoint, "vel"),
make_protocol_function("set_velocity_limit", *this, &CtrlStepMotor::SetVelocityLimit, "vel"),
make_protocol_function("set_current", *this, &CtrlStepMotor::SetCurrentSetPoint, "current"),
make_protocol_function("set_current_limit", *this, &CtrlStepMotor::SetCurrentLimit, "current"),
make_protocol_function("set_node_id", *this, &CtrlStepMotor::SetNodeID, "id"),
make_protocol_function("set_acceleration", *this, &CtrlStepMotor::SetAcceleration, "acc",
"stored"),
make_protocol_function("apply_home_offset", *this, &CtrlStepMotor::ApplyPositionAsHome),
make_protocol_function("do_calibration", *this, &CtrlStepMotor::DoCalibration),
make_protocol_function("set_enable_on_boot", *this, &CtrlStepMotor::SetEnableOnBoot, "enable"),
make_protocol_function("set_dce_kp", *this, &CtrlStepMotor::SetDceKp, "vel"),
make_protocol_function("set_dce_kv", *this, &CtrlStepMotor::SetDceKv, "vel"),
make_protocol_function("set_dce_ki", *this, &CtrlStepMotor::SetDceKi, "vel"),
make_protocol_function("set_dce_kd", *this, &CtrlStepMotor::SetDceKd, "vel"),
make_protocol_function("set_enable_ack", *this, &CtrlStepMotor::SetEnableAck, "enable"),
make_protocol_function("set_enable_stall_protect", *this, &CtrlStepMotor::SetEnableStallProtect,
"enable"),
make_protocol_function("update_angle", *this, &CtrlStepMotor::UpdateAngle)
);
}
private:
CAN_HandleTypeDef* hcan;
uint8_t canBuf[8] = {};
CAN_TxHeaderTypeDef txHeader = {};
};
#endif //DUMMY_CORE_FW_CTRL_STEP_HPP
#ifndef DUMMY_CORE_FW_CTRL_STEP_HPP
#define DUMMY_CORE_FW_CTRL_STEP_HPP
#include "fibre/protocol.hpp"
#include "can.h"
class CtrlStepMotor
{
public:
enum State
{
RUNNING,
FINISH,
STOP
};
const uint32_t CTRL_CIRCLE_COUNT = 200 * 256;
CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse = false, uint8_t _reduction = 1,
float _angleLimitMin = -180, float _angleLimitMax = 180);
uint8_t nodeID;
float angle = 0;
float angleLimitMax;
float angleLimitMin;
bool inverseDirection;
uint8_t reduction;
State state = STOP;
void SetAngle(float _angle);
void SetAngleWithVelocityLimit(float _angle, float _vel);
// CAN Command
void SetEnable(bool _enable);
void DoCalibration();
void SetCurrentSetPoint(float _val);
void SetVelocitySetPoint(float _val);
void SetPositionSetPoint(float _val);
void SetPositionWithVelocityLimit(float _pos, float _vel);
void SetNodeID(uint32_t _id);
void SetCurrentLimit(float _val);
void SetVelocityLimit(float _val);
void SetAcceleration(float _val);
void SetDceKp(int32_t _val);
void SetDceKv(int32_t _val);
void SetDceKi(int32_t _val);
void SetDceKd(int32_t _val);
void ApplyPositionAsHome();
void SetEnableOnBoot(bool _enable);
void SetEnableStallProtect(bool _enable);
void Reboot();
void EraseConfigs();
void UpdateAngle();
void UpdateAngleCallback(float _pos, bool _isFinished);
// Communication protocol definitions
auto MakeProtocolDefinitions()
{
return make_protocol_member_list(
make_protocol_ro_property("angle", &angle),
make_protocol_function("reboot", *this, &CtrlStepMotor::Reboot),
make_protocol_function("erase_configs", *this, &CtrlStepMotor::EraseConfigs),
make_protocol_function("set_enable", *this, &CtrlStepMotor::SetEnable, "enable"),
make_protocol_function("set_position_with_time", *this,
&CtrlStepMotor::SetPositionWithVelocityLimit, "pos", "time"),
make_protocol_function("set_position", *this, &CtrlStepMotor::SetPositionSetPoint, "pos"),
make_protocol_function("set_velocity", *this, &CtrlStepMotor::SetVelocitySetPoint, "vel"),
make_protocol_function("set_velocity_limit", *this, &CtrlStepMotor::SetVelocityLimit, "vel"),
make_protocol_function("set_current", *this, &CtrlStepMotor::SetCurrentSetPoint, "current"),
make_protocol_function("set_current_limit", *this, &CtrlStepMotor::SetCurrentLimit, "current"),
make_protocol_function("set_node_id", *this, &CtrlStepMotor::SetNodeID, "id"),
make_protocol_function("set_acceleration", *this, &CtrlStepMotor::SetAcceleration, "acc"),
make_protocol_function("apply_home_offset", *this, &CtrlStepMotor::ApplyPositionAsHome),
make_protocol_function("do_calibration", *this, &CtrlStepMotor::DoCalibration),
make_protocol_function("set_enable_on_boot", *this, &CtrlStepMotor::SetEnableOnBoot, "enable"),
make_protocol_function("set_dce_kp", *this, &CtrlStepMotor::SetDceKp, "vel"),
make_protocol_function("set_dce_kv", *this, &CtrlStepMotor::SetDceKv, "vel"),
make_protocol_function("set_dce_ki", *this, &CtrlStepMotor::SetDceKi, "vel"),
make_protocol_function("set_dce_kd", *this, &CtrlStepMotor::SetDceKd, "vel"),
make_protocol_function("set_enable_stall_protect", *this, &CtrlStepMotor::SetEnableStallProtect,
"enable"),
make_protocol_function("update_angle", *this, &CtrlStepMotor::UpdateAngle)
);
}
private:
CAN_HandleTypeDef* hcan;
uint8_t canBuf[8] = {};
CAN_TxHeaderTypeDef txHeader = {};
};
#endif //DUMMY_CORE_FW_CTRL_STEP_HPP

View File

@ -1,83 +1,83 @@
#ifndef DOF6_KINEMATIC_SOLVER_H
#define DOF6_KINEMATIC_SOLVER_H
#include "stm32f405xx.h"
#include "arm_math.h"
#include "memory.h"
class DOF6Kinematic
{
private:
const float RAD_TO_DEG = 57.295777754771045f;
// DH parameters
struct ArmConfig_t
{
float L_BASE;
float D_BASE;
float L_ARM;
float L_FOREARM;
float D_ELBOW;
float L_WRIST;
};
ArmConfig_t armConfig;
float DH_matrix[6][4] = {0}; // home,d,a,alpha
float L1_base[3] = {0};
float L2_arm[3] = {0};
float L3_elbow[3] = {0};
float L6_wrist[3] = {0};
float l_se_2;
float l_se;
float l_ew_2;
float l_ew;
float atan_e;
public:
struct Joint6D_t
{
Joint6D_t()
= default;
Joint6D_t(float a1, float a2, float a3, float a4, float a5, float a6)
: a{a1, a2, a3, a4, a5, a6}
{}
float a[6];
friend Joint6D_t operator-(const Joint6D_t &_joints1, const Joint6D_t &_joints2);
};
struct Pose6D_t
{
Pose6D_t()
= default;
Pose6D_t(float x, float y, float z, float a, float b, float c)
: X(x), Y(y), Z(z), A(a), B(b), C(c), hasR(false)
{}
float X{}, Y{}, Z{};
float A{}, B{}, C{};
float R[9]{};
// if Pose was calculated by FK then it's true automatically (so that no need to do extra calc),
// otherwise if manually set params then it should be set to false.
bool hasR{};
};
struct IKSolves_t
{
Joint6D_t config[8];
char solFlag[8][3];
};
DOF6Kinematic(float L_BS, float D_BS, float L_SE, float L_EW, float D_EW, float L_WT);
bool SolveFK(const Joint6D_t &_inputJoint6D, Pose6D_t &_outputPose6D);
bool SolveIK(const Pose6D_t &_inputPose6D, const Joint6D_t &_lastJoint6D, IKSolves_t &_outputSolves);
};
#endif //DOF6_KINEMATIC_SOLVER_H
#ifndef DOF6_KINEMATIC_SOLVER_H
#define DOF6_KINEMATIC_SOLVER_H
#include "stm32f405xx.h"
#include "arm_math.h"
#include "memory.h"
class DOF6Kinematic
{
private:
const float RAD_TO_DEG = 57.295777754771045f;
// DH parameters
struct ArmConfig_t
{
float L_BASE;
float D_BASE;
float L_ARM;
float L_FOREARM;
float D_ELBOW;
float L_WRIST;
};
ArmConfig_t armConfig;
float DH_matrix[6][4] = {0}; // home,d,a,alpha
float L1_base[3] = {0};
float L2_arm[3] = {0};
float L3_elbow[3] = {0};
float L6_wrist[3] = {0};
float l_se_2;
float l_se;
float l_ew_2;
float l_ew;
float atan_e;
public:
struct Joint6D_t
{
Joint6D_t()
= default;
Joint6D_t(float a1, float a2, float a3, float a4, float a5, float a6)
: a{a1, a2, a3, a4, a5, a6}
{}
float a[6];
friend Joint6D_t operator-(const Joint6D_t &_joints1, const Joint6D_t &_joints2);
};
struct Pose6D_t
{
Pose6D_t()
= default;
Pose6D_t(float x, float y, float z, float a, float b, float c)
: X(x), Y(y), Z(z), A(a), B(b), C(c), hasR(false)
{}
float X{}, Y{}, Z{};
float A{}, B{}, C{};
float R[9]{};
// if Pose was calculated by FK then it's true automatically (so that no need to do extra calc),
// otherwise if manually set params then it should be set to false.
bool hasR{};
};
struct IKSolves_t
{
Joint6D_t config[8];
char solFlag[8][3];
};
DOF6Kinematic(float L_BS, float D_BS, float L_AM, float L_FA, float D_EW, float L_WT);
bool SolveFK(const Joint6D_t &_inputJoint6D, Pose6D_t &_outputPose6D);
bool SolveIK(const Pose6D_t &_inputPose6D, const Joint6D_t &_lastJoint6D, IKSolves_t &_outputSolves);
};
#endif //DOF6_KINEMATIC_SOLVER_H

File diff suppressed because it is too large Load Diff

View File

@ -1,167 +1,205 @@
#ifndef REF_STM32F4_FW_DUMMY_ROBOT_H
#define REF_STM32F4_FW_DUMMY_ROBOT_H
#include "algorithms/kinematic/6dof_kinematic.h"
#include "actuators/ctrl_step/ctrl_step.hpp"
#define ALL 0
/*
| PARAMS | `current_limit` | `acceleration` | `dce_kp` | `dce_kv` | `dce_ki` | `dce_kd` |
| ---------- | --------------- | -------------- | -------- | -------- | -------- | -------- |
| **Joint1** | 1 | 25 | 1000 | 80 | 300 | 250 |
| **Joint2** | 1.5 | 25 | 1000 | 250 | 300 | 200 |
| **Joint3** | 1 | 25 | 1000 | 350 | 500 | 250 |
| **Joint4** | 1.5 | 25 | 1000 | 350 | 500 | 250 |
| **Joint5** | 1.5 | 25 | 1000 | 350 | 500 | 250 |
| **Joint6** | 1.5 | 25 | 1000 | 350 | 500 | 250 |
*/
class DummyHand
{
public:
uint8_t nodeID = 7;
float maxCurrent = 0.7;
DummyHand(CAN_HandleTypeDef* _hcan, uint8_t _id);
void SetAngle(float _angle);
void SetMaxCurrent(float _val);
void SetEnable(bool _enable);
// Communication protocol definitions
auto MakeProtocolDefinitions()
{
return make_protocol_member_list(
make_protocol_function("set_angle", *this, &DummyHand::SetAngle, "angle"),
make_protocol_function("set_enable", *this, &DummyHand::SetEnable, "enable"),
make_protocol_function("set_current_limit", *this, &DummyHand::SetMaxCurrent, "current")
);
}
private:
CAN_HandleTypeDef* hcan;
uint8_t canBuf[8];
CAN_TxHeaderTypeDef txHeader;
float minAngle = 0;
float maxAngle = 45;
};
class DummyRobot
{
public:
explicit DummyRobot(CAN_HandleTypeDef* _hcan);
~DummyRobot();
enum CommandMode
{
COMMAND_TARGET_POINT_SEQUENTIAL = 1,
COMMAND_TARGET_POINT_INTERRUPTABLE,
COMMAND_CONTINUES_TRAJECTORY
};
// This is the pose when power on.
const DOF6Kinematic::Joint6D_t REST_POSE = {0, -73, 180, 0, 0, 0};
const float DEFAULT_JOINT_SPEED = 30;
const float DEFAULT_JOINT_ACCELERATION = 50;
DOF6Kinematic::Joint6D_t currentJoints = REST_POSE;
DOF6Kinematic::Joint6D_t initPose = REST_POSE;
DOF6Kinematic::Pose6D_t currentPose6D = {};
volatile uint8_t jointsStateFlag = 0b00000000;
CommandMode commandMode = COMMAND_TARGET_POINT_SEQUENTIAL;
bool isStopped = false;
CtrlStepMotor* motorJ[7] = {nullptr};
DummyHand* hand = {nullptr};
float MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6);
float MoveL(float _x, float _y, float _z, float _a, float _b, float _c);
void MoveTrajectoryJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6);
void MoveTrajectoryL(float _x, float _y, float _z, float _a, float _b, float _c);
void SetJointSpeed(float _speed);
void SetJointAcceleration(float _acc);
void UpdateJointAngles();
void UpdateJointAnglesCallback();
void UpdateJointPose6D();
void Reboot();
void SetEnable(bool _enable);
void CalibrateHomeOffset();
void Homing();
void Resting();
bool IsMoving();
void SetCommandMode(uint8_t _mode);
// Communication protocol definitions
auto MakeProtocolDefinitions()
{
return make_protocol_member_list(
make_protocol_function("calibrate_home_offset", *this, &DummyRobot::CalibrateHomeOffset),
make_protocol_function("homing", *this, &DummyRobot::Homing),
make_protocol_function("resting", *this, &DummyRobot::Resting),
make_protocol_object("joint_1", motorJ[1]->MakeProtocolDefinitions()),
make_protocol_object("joint_2", motorJ[2]->MakeProtocolDefinitions()),
make_protocol_object("joint_3", motorJ[3]->MakeProtocolDefinitions()),
make_protocol_object("joint_4", motorJ[4]->MakeProtocolDefinitions()),
make_protocol_object("joint_5", motorJ[5]->MakeProtocolDefinitions()),
make_protocol_object("joint_6", motorJ[6]->MakeProtocolDefinitions()),
make_protocol_object("joint_all", motorJ[ALL]->MakeProtocolDefinitions()),
make_protocol_object("hand", hand->MakeProtocolDefinitions()),
make_protocol_function("reboot", *this, &DummyRobot::Reboot),
make_protocol_function("set_enable", *this, &DummyRobot::SetEnable, "enable"),
make_protocol_function("move_j", *this, &DummyRobot::MoveJ, "j1", "j2", "j3", "j4", "j5", "j6"),
make_protocol_function("move_l", *this, &DummyRobot::MoveL, "x", "y", "z", "a", "b", "c"),
make_protocol_function("set_joint_speed", *this, &DummyRobot::SetJointSpeed, "speed"),
make_protocol_function("set_joint_acc", *this, &DummyRobot::SetJointAcceleration, "acc"),
make_protocol_function("set_command_mode", *this, &DummyRobot::SetCommandMode, "mode")
);
}
class CommandHandler
{
public:
explicit CommandHandler(DummyRobot* _context) : context(_context)
{
commandFifo = osMessageQueueNew(16, 64, nullptr);
commandLifo = osMessageQueueNew(1, 64, nullptr);
}
uint32_t Push(const std::string &_cmd);
std::string Pop(uint32_t timeout);
uint32_t ParseCommand(const std::string &_cmd);
uint32_t GetSpace();
void ClearFifo();
void EmergencyStop();
void Resume();
private:
DummyRobot* context;
osMessageQueueId_t commandFifo;
osMessageQueueId_t commandLifo;
char strBuffer[64]{};
};
CommandHandler commandHandler = CommandHandler(this);
private:
CAN_HandleTypeDef* hcan;
float jointSpeed = DEFAULT_JOINT_SPEED;
DOF6Kinematic* dof6Solver;
float MoveJoints(DOF6Kinematic::Joint6D_t _joints);
};
#endif //REF_STM32F4_FW_DUMMY_ROBOT_H
#ifndef REF_STM32F4_FW_DUMMY_ROBOT_H
#define REF_STM32F4_FW_DUMMY_ROBOT_H
#include "algorithms/kinematic/6dof_kinematic.h"
#include "actuators/ctrl_step/ctrl_step.hpp"
#define ALL 0
/*
| PARAMS | `current_limit` | `acceleration` | `dce_kp` | `dce_kv` | `dce_ki` | `dce_kd` |
| ---------- | --------------- | -------------- | -------- | -------- | -------- | -------- |
| **Joint1** | 2 | 30 | 1000 | 80 | 200 | 250 |
| **Joint2** | 2 | 30 | 1000 | 80 | 200 | 200 |
| **Joint3** | 2 | 30 | 1500 | 80 | 200 | 250 |
| **Joint4** | 2 | 30 | 1000 | 80 | 200 | 250 |
| **Joint5** | 2 | 30 | 1000 | 80 | 200 | 250 |
| **Joint6** | 2 | 30 | 1000 | 80 | 200 | 250 |
*/
class DummyHand
{
public:
uint8_t nodeID = 7;
float maxCurrent = 0.7;
DummyHand(CAN_HandleTypeDef* _hcan, uint8_t _id);
void SetAngle(float _angle);
void SetMaxCurrent(float _val);
void SetEnable(bool _enable);
// Communication protocol definitions
auto MakeProtocolDefinitions()
{
return make_protocol_member_list(
make_protocol_function("set_angle", *this, &DummyHand::SetAngle, "angle"),
make_protocol_function("set_enable", *this, &DummyHand::SetEnable, "enable"),
make_protocol_function("set_current_limit", *this, &DummyHand::SetMaxCurrent, "current")
);
}
private:
CAN_HandleTypeDef* hcan;
uint8_t canBuf[8];
CAN_TxHeaderTypeDef txHeader;
float minAngle = 0;
float maxAngle = 45;
};
class DummyRobot
{
public:
explicit DummyRobot(CAN_HandleTypeDef* _hcan);
~DummyRobot();
enum CommandMode
{
COMMAND_TARGET_POINT_SEQUENTIAL = 1,
COMMAND_TARGET_POINT_INTERRUPTABLE,
COMMAND_CONTINUES_TRAJECTORY,
COMMAND_MOTOR_TUNING
};
class TuningHelper
{
public:
explicit TuningHelper(DummyRobot* _context) : context(_context)
{
}
void SetTuningFlag(uint8_t _flag);
void Tick(uint32_t _timeMillis);
void SetFreqAndAmp(float _freq, float _amp);
// Communication protocol definitions
auto MakeProtocolDefinitions()
{
return make_protocol_member_list(
make_protocol_function("set_tuning_freq_amp", *this,
&TuningHelper::SetFreqAndAmp, "freq", "amp"),
make_protocol_function("set_tuning_flag", *this,
&TuningHelper::SetTuningFlag, "flag")
);
}
private:
DummyRobot* context;
float time = 0;
uint8_t tuningFlag = 0;
float frequency = 1;
float amplitude = 1;
};
TuningHelper tuningHelper = TuningHelper(this);
// This is the pose when power on.
const DOF6Kinematic::Joint6D_t REST_POSE = {0, -73, 180, 0, 0, 0};
const float DEFAULT_JOINT_SPEED = 30; // degree/s
const DOF6Kinematic::Joint6D_t DEFAULT_JOINT_ACCELERATION_BASES = {150, 100, 200, 200, 200, 200};
const float DEFAULT_JOINT_ACCELERATION_LOW = 30; // 0~100
const float DEFAULT_JOINT_ACCELERATION_HIGH = 100; // 0~100
const CommandMode DEFAULT_COMMAND_MODE = COMMAND_TARGET_POINT_INTERRUPTABLE;
DOF6Kinematic::Joint6D_t currentJoints = REST_POSE;
DOF6Kinematic::Joint6D_t targetJoints = REST_POSE;
DOF6Kinematic::Joint6D_t initPose = REST_POSE;
DOF6Kinematic::Pose6D_t currentPose6D = {};
volatile uint8_t jointsStateFlag = 0b00000000;
CommandMode commandMode = DEFAULT_COMMAND_MODE;
CtrlStepMotor* motorJ[7] = {nullptr};
DummyHand* hand = {nullptr};
void Init();
bool MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6);
bool MoveL(float _x, float _y, float _z, float _a, float _b, float _c);
void MoveJoints(DOF6Kinematic::Joint6D_t _joints);
void SetJointSpeed(float _speed);
void SetJointAcceleration(float _acc);
void UpdateJointAngles();
void UpdateJointAnglesCallback();
void UpdateJointPose6D();
void Reboot();
void SetEnable(bool _enable);
void CalibrateHomeOffset();
void Homing();
void Resting();
bool IsMoving();
bool IsEnabled();
void SetCommandMode(uint32_t _mode);
// Communication protocol definitions
auto MakeProtocolDefinitions()
{
return make_protocol_member_list(
make_protocol_function("calibrate_home_offset", *this, &DummyRobot::CalibrateHomeOffset),
make_protocol_function("homing", *this, &DummyRobot::Homing),
make_protocol_function("resting", *this, &DummyRobot::Resting),
make_protocol_object("joint_1", motorJ[1]->MakeProtocolDefinitions()),
make_protocol_object("joint_2", motorJ[2]->MakeProtocolDefinitions()),
make_protocol_object("joint_3", motorJ[3]->MakeProtocolDefinitions()),
make_protocol_object("joint_4", motorJ[4]->MakeProtocolDefinitions()),
make_protocol_object("joint_5", motorJ[5]->MakeProtocolDefinitions()),
make_protocol_object("joint_6", motorJ[6]->MakeProtocolDefinitions()),
make_protocol_object("joint_all", motorJ[ALL]->MakeProtocolDefinitions()),
make_protocol_object("hand", hand->MakeProtocolDefinitions()),
make_protocol_function("reboot", *this, &DummyRobot::Reboot),
make_protocol_function("set_enable", *this, &DummyRobot::SetEnable, "enable"),
make_protocol_function("move_j", *this, &DummyRobot::MoveJ, "j1", "j2", "j3", "j4", "j5", "j6"),
make_protocol_function("move_l", *this, &DummyRobot::MoveL, "x", "y", "z", "a", "b", "c"),
make_protocol_function("set_joint_speed", *this, &DummyRobot::SetJointSpeed, "speed"),
make_protocol_function("set_joint_acc", *this, &DummyRobot::SetJointAcceleration, "acc"),
make_protocol_function("set_command_mode", *this, &DummyRobot::SetCommandMode, "mode"),
make_protocol_object("tuning", tuningHelper.MakeProtocolDefinitions())
);
}
class CommandHandler
{
public:
explicit CommandHandler(DummyRobot* _context) : context(_context)
{
commandFifo = osMessageQueueNew(16, 64, nullptr);
}
uint32_t Push(const std::string &_cmd);
std::string Pop(uint32_t timeout);
uint32_t ParseCommand(const std::string &_cmd);
uint32_t GetSpace();
void ClearFifo();
void EmergencyStop();
private:
DummyRobot* context;
osMessageQueueId_t commandFifo;
char strBuffer[64]{};
};
CommandHandler commandHandler = CommandHandler(this);
private:
CAN_HandleTypeDef* hcan;
float jointSpeed = DEFAULT_JOINT_SPEED;
float jointSpeedRatio = 1;
DOF6Kinematic::Joint6D_t dynamicJointSpeeds = {1, 1, 1, 1, 1, 1};
DOF6Kinematic* dof6Solver;
bool isEnabled = false;
};
#endif //REF_STM32F4_FW_DUMMY_ROBOT_H

View File

@ -1,164 +1,183 @@
#include "common_inc.h"
// On-board Screen, can choose from hi2c2 or hi2c0(soft i2c)
SSD1306 oled(&hi2c0);
// On-board Sensor, used hi2c1
MPU6050 mpu6050(&hi2c1);
// 5 User-Timers, can choose from htim7/htim10/htim11/htim13/htim14
Timer timerCtrlLoop(&htim7, 200);
// 2x2-channel PWMs, used htim9 & htim12, each has 2-channel outputs
PWM pwm(21000, 21000);
// Robot instance
DummyRobot dummy(&hcan1);
/* Thread Definitions -----------------------------------------------------*/
osThreadId_t controlLoopFixUpdateHandle;
void ThreadControlLoopFixUpdate(void* argument)
{
for (;;)
{
// Suspended here until got Notification.
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
switch (dummy.commandMode)
{
case DummyRobot::COMMAND_TARGET_POINT_SEQUENTIAL:
case DummyRobot::COMMAND_TARGET_POINT_INTERRUPTABLE:
dummy.UpdateJointAngles();
dummy.UpdateJointPose6D();
break;
case DummyRobot::COMMAND_CONTINUES_TRAJECTORY:
// ToDo: handle trajectory, while update state at the mean time
dummy.UpdateJointPose6D();
break;
}
}
}
osThreadId_t ControlLoopUpdateHandle;
void ThreadControlLoopUpdate(void* argument)
{
for (;;)
{
dummy.commandHandler.ParseCommand(dummy.commandHandler.Pop(osWaitForever));
}
}
osThreadId_t oledTaskHandle;
void ThreadOledUpdate(void* argument)
{
uint32_t t = micros();
char buf[16];
char cmdModeNames[3][4] = {"SEQ", "INT", "TRJ"};
for (;;)
{
mpu6050.Update(true);
oled.clearBuffer();
oled.setFont(u8g2_font_5x8_tr);
oled.setCursor(0, 10);
oled.printf("IMU:%.3f/%.3f", mpu6050.data.ax, mpu6050.data.ay);
oled.setCursor(85, 10);
oled.printf("| FPS:%lu", 1000000 / (micros() - t));
t = micros();
oled.drawBox(0, 15, 128, 3);
oled.setCursor(0, 30);
oled.printf(">%3d|%3d|%3d|%3d|%3d|%3d",
(int) roundf(dummy.currentJoints.a[0]), (int) roundf(dummy.currentJoints.a[1]),
(int) roundf(dummy.currentJoints.a[2]), (int) roundf(dummy.currentJoints.a[3]),
(int) roundf(dummy.currentJoints.a[4]), (int) roundf(dummy.currentJoints.a[5]));
oled.drawBox(40, 35, 128, 24);
oled.setFont(u8g2_font_6x12_tr);
oled.setDrawColor(0);
oled.setCursor(42, 45);
oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.X),
(int) roundf(dummy.currentPose6D.Y), (int) roundf(dummy.currentPose6D.Z));
oled.setCursor(42, 56);
oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.A),
(int) roundf(dummy.currentPose6D.B), (int) roundf(dummy.currentPose6D.C));
oled.setDrawColor(1);
oled.setCursor(0, 45);
oled.printf("[XYZ]:");
oled.setCursor(0, 56);
oled.printf("[ABC]:");
oled.setFont(u8g2_font_10x20_tr);
oled.setCursor(0, 78);
for (int i = 1; i <= 6; i++)
buf[i - 1] = (dummy.jointsStateFlag & (1 << i) ? '*' : '_');
buf[6] = 0;
oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], buf);
oled.sendBuffer();
}
}
/* Timer Callbacks -------------------------------------------------------*/
void OnTimer7Callback()
{
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
// Wake & invoke thread IMMEDIATELY.
vTaskNotifyGiveFromISR(TaskHandle_t(controlLoopFixUpdateHandle), &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
/* Default Entry -------------------------------------------------------*/
void Main(void)
{
// Init all communication staff, include USB-CDC/VCP/UART/CAN etc.
InitCommunication();
// Init IMU.
do
{
mpu6050.Init();
osDelay(100);
} while (!mpu6050.testConnection());
mpu6050.InitFilter(200, 100, 50);
// Init OLED 128x80.
oled.Init();
pwm.Start();
// Init & Run User Threads.
const osThreadAttr_t controlLoopTask_attributes = {
.name = "ControlLoopFixUpdateTask",
.stack_size = 1000 * 4,
.priority = (osPriority_t) osPriorityRealtime,
};
controlLoopFixUpdateHandle = osThreadNew(ThreadControlLoopFixUpdate, nullptr,
&controlLoopTask_attributes);
const osThreadAttr_t ControlLoopUpdateTask_attributes = {
.name = "ControlLoopUpdateTask",
.stack_size = 1000 * 4,
.priority = (osPriority_t) osPriorityNormal,
};
ControlLoopUpdateHandle = osThreadNew(ThreadControlLoopUpdate, nullptr,
&ControlLoopUpdateTask_attributes);
const osThreadAttr_t oledTask_attributes = {
.name = "OledTask",
.stack_size = 1000 * 4,
.priority = (osPriority_t) osPriorityNormal, // should >= Normal
};
oledTaskHandle = osThreadNew(ThreadOledUpdate, nullptr, &oledTask_attributes);
// Start Timer Callbacks.
timerCtrlLoop.SetCallback(OnTimer7Callback);
timerCtrlLoop.Start();
// System started, light switch-led up.
pwm.SetDuty(PWM::CH_A1, 0.5);
}
#include "common_inc.h"
// On-board Screen, can choose from hi2c2 or hi2c0(soft i2c)
SSD1306 oled(&hi2c0);
// On-board Sensor, used hi2c1
MPU6050 mpu6050(&hi2c1);
// 5 User-Timers, can choose from htim7/htim10/htim11/htim13/htim14
Timer timerCtrlLoop(&htim7, 200);
// 2x2-channel PWMs, used htim9 & htim12, each has 2-channel outputs
PWM pwm(21000, 21000);
// Robot instance
DummyRobot dummy(&hcan1);
/* Thread Definitions -----------------------------------------------------*/
osThreadId_t controlLoopFixUpdateHandle;
void ThreadControlLoopFixUpdate(void* argument)
{
for (;;)
{
// Suspended here until got Notification.
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
if (dummy.IsEnabled())
{
// Send control command to Motors & update Joint states
switch (dummy.commandMode)
{
case DummyRobot::COMMAND_TARGET_POINT_SEQUENTIAL:
case DummyRobot::COMMAND_TARGET_POINT_INTERRUPTABLE:
case DummyRobot::COMMAND_CONTINUES_TRAJECTORY:
dummy.MoveJoints(dummy.targetJoints);
dummy.UpdateJointPose6D();
break;
case DummyRobot::COMMAND_MOTOR_TUNING:
dummy.tuningHelper.Tick(10);
dummy.UpdateJointPose6D();
break;
}
} else
{
// Just update Joint states
dummy.UpdateJointAngles();
dummy.UpdateJointPose6D();
}
}
}
osThreadId_t ControlLoopUpdateHandle;
void ThreadControlLoopUpdate(void* argument)
{
for (;;)
{
dummy.commandHandler.ParseCommand(dummy.commandHandler.Pop(osWaitForever));
}
}
osThreadId_t oledTaskHandle;
void ThreadOledUpdate(void* argument)
{
uint32_t t = micros();
char buf[16];
char cmdModeNames[4][4] = {"SEQ", "INT", "TRJ", "TUN"};
for (;;)
{
mpu6050.Update(true);
oled.clearBuffer();
oled.setFont(u8g2_font_5x8_tr);
oled.setCursor(0, 10);
oled.printf("IMU:%.3f/%.3f", mpu6050.data.ax, mpu6050.data.ay);
oled.setCursor(85, 10);
oled.printf("| FPS:%lu", 1000000 / (micros() - t));
t = micros();
oled.drawBox(0, 15, 128, 3);
oled.setCursor(0, 30);
oled.printf(">%3d|%3d|%3d|%3d|%3d|%3d",
(int) roundf(dummy.currentJoints.a[0]), (int) roundf(dummy.currentJoints.a[1]),
(int) roundf(dummy.currentJoints.a[2]), (int) roundf(dummy.currentJoints.a[3]),
(int) roundf(dummy.currentJoints.a[4]), (int) roundf(dummy.currentJoints.a[5]));
oled.drawBox(40, 35, 128, 24);
oled.setFont(u8g2_font_6x12_tr);
oled.setDrawColor(0);
oled.setCursor(42, 45);
oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.X),
(int) roundf(dummy.currentPose6D.Y), (int) roundf(dummy.currentPose6D.Z));
oled.setCursor(42, 56);
oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.A),
(int) roundf(dummy.currentPose6D.B), (int) roundf(dummy.currentPose6D.C));
oled.setDrawColor(1);
oled.setCursor(0, 45);
oled.printf("[XYZ]:");
oled.setCursor(0, 56);
oled.printf("[ABC]:");
oled.setFont(u8g2_font_10x20_tr);
oled.setCursor(0, 78);
if (dummy.IsEnabled())
{
for (int i = 1; i <= 6; i++)
buf[i - 1] = (dummy.jointsStateFlag & (1 << i) ? '*' : '_');
buf[6] = 0;
oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], buf);
} else
{
oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], "======");
}
oled.sendBuffer();
}
}
/* Timer Callbacks -------------------------------------------------------*/
void OnTimer7Callback()
{
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
// Wake & invoke thread IMMEDIATELY.
vTaskNotifyGiveFromISR(TaskHandle_t(controlLoopFixUpdateHandle), &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
/* Default Entry -------------------------------------------------------*/
void Main(void)
{
// Init all communication staff, including USB-CDC/VCP/UART/CAN etc.
InitCommunication();
// Init Robot.
dummy.Init();
// Init IMU.
do
{
mpu6050.Init();
osDelay(100);
} while (!mpu6050.testConnection());
mpu6050.InitFilter(200, 100, 50);
// Init OLED 128x80.
oled.Init();
pwm.Start();
// Init & Run User Threads.
const osThreadAttr_t controlLoopTask_attributes = {
.name = "ControlLoopFixUpdateTask",
.stack_size = 2000,
.priority = (osPriority_t) osPriorityRealtime,
};
controlLoopFixUpdateHandle = osThreadNew(ThreadControlLoopFixUpdate, nullptr,
&controlLoopTask_attributes);
const osThreadAttr_t ControlLoopUpdateTask_attributes = {
.name = "ControlLoopUpdateTask",
.stack_size = 2000,
.priority = (osPriority_t) osPriorityNormal,
};
ControlLoopUpdateHandle = osThreadNew(ThreadControlLoopUpdate, nullptr,
&ControlLoopUpdateTask_attributes);
const osThreadAttr_t oledTask_attributes = {
.name = "OledTask",
.stack_size = 2000,
.priority = (osPriority_t) osPriorityNormal, // should >= Normal
};
oledTaskHandle = osThreadNew(ThreadOledUpdate, nullptr, &oledTask_attributes);
// Start Timer Callbacks.
timerCtrlLoop.SetCallback(OnTimer7Callback);
timerCtrlLoop.Start();
// System started, light switch-led up.
Respond(*uart4StreamOutputPtr, "[sys] Heap remain: %d Bytes\n", xPortGetMinimumEverFreeHeapSize());
pwm.SetDuty(PWM::CH_A1, 0.5);
}

View File

@ -1,123 +1,116 @@
#include "common_inc.h"
extern DummyRobot dummy;
void OnUsbAsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
{
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
if (_cmd[0] == '!' || dummy.isStopped)
{
std::string s(_cmd);
if (s.find("STOP") != std::string::npos)
{
dummy.commandHandler.EmergencyStop();
Respond(_responseChannel, "!!!Stopped!!!");
} else if (s.find("RESUME") != std::string::npos)
{
dummy.commandHandler.Resume();
Respond(_responseChannel, "Resumed");
}
} else if (_cmd[0] == '#')
{
std::string s(_cmd);
if (s.find("GETJPOS") != std::string::npos)
{
Respond(*usbStreamOutputPtr, "JNTS %.2f %.2f %.2f %.2f %.2f %.2f",
dummy.currentJoints.a[0], dummy.currentJoints.a[1],
dummy.currentJoints.a[2], dummy.currentJoints.a[3],
dummy.currentJoints.a[4], dummy.currentJoints.a[5]);
} else if (s.find("GETLPOS") != std::string::npos)
{
dummy.UpdateJointPose6D();
Respond(*usbStreamOutputPtr, "POSE %.2f %.2f %.2f %.2f %.2f %.2f",
dummy.currentPose6D.X, dummy.currentPose6D.Y,
dummy.currentPose6D.Z, dummy.currentPose6D.A,
dummy.currentPose6D.B, dummy.currentPose6D.C);
} else if (s.find("CMDMODE") != std::string::npos)
{
int mode;
sscanf(_cmd, "CMDMODE %d", &mode);
dummy.SetCommandMode(mode);
Respond(*usbStreamOutputPtr, "Set command mode to [%d]", mode);
} else
Respond(*usbStreamOutputPtr, "ok");
} else if (_cmd[0] == '>' || _cmd[0] == '@')
{
uint32_t freeSize = dummy.commandHandler.Push(_cmd);
Respond(_responseChannel, "%d", freeSize);
}
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
}
void OnUart4AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
{
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
uint8_t argNum;
if (_cmd[0] == '#')
{
std::string s(_cmd);
if (s.find("GETJPOS") != std::string::npos)
{
Respond(_responseChannel, "JNTS %.2f %.2f %.2f %.2f %.2f %.2f",
dummy.currentJoints.a[0], dummy.currentJoints.a[1], dummy.currentJoints.a[2],
dummy.currentJoints.a[3], dummy.currentJoints.a[4], dummy.currentJoints.a[5]);
} else
Respond(_responseChannel, "ok");
} else if (_cmd[0] == '>')
{
float joints[6];
float speed;
argNum = sscanf(_cmd, ">%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2,
joints + 3, joints + 4, joints + 5, &speed);
if (argNum == 6)
{
dummy.MoveJ(joints[0], joints[1], joints[2],
joints[3], joints[4], joints[5]);
} else if (argNum == 7)
{
dummy.SetJointSpeed(speed);
dummy.MoveJ(joints[0], joints[1], joints[2],
joints[3], joints[4], joints[5]);
}
while (dummy.IsMoving())
osDelay(10);
Respond(_responseChannel, "ok");
} else if (_cmd[0] == '@')
{
float pose[6];
float speed;
argNum = sscanf(_cmd, "@%f,%f,%f,%f,%f,%f,%f", pose, pose + 1, pose + 2,
pose + 3, pose + 4, pose + 5, &speed);
if (argNum == 6)
{
dummy.MoveL(pose[0], pose[1], pose[2],
pose[3], pose[4], pose[5]);
} else if (argNum == 7)
{
dummy.SetJointSpeed(speed);
dummy.MoveL(pose[0], pose[1], pose[2],
pose[3], pose[4], pose[5]);
}
while (dummy.IsMoving())
osDelay(10);
Respond(_responseChannel, "ok");
}
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
}
void OnUart5AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
{
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
#include "common_inc.h"
extern DummyRobot dummy;
void OnUsbAsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
{
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
if (_cmd[0] == '!' || !dummy.IsEnabled())
{
std::string s(_cmd);
if (s.find("STOP") != std::string::npos)
{
dummy.commandHandler.EmergencyStop();
Respond(_responseChannel, "Stopped ok");
} else if (s.find("START") != std::string::npos)
{
dummy.SetEnable(true);
Respond(_responseChannel, "Started ok");
} else if (s.find("DISABLE") != std::string::npos)
{
dummy.SetEnable(false);
Respond(_responseChannel, "Disabled ok");
}
} else if (_cmd[0] == '#')
{
std::string s(_cmd);
if (s.find("GETJPOS") != std::string::npos)
{
Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
dummy.currentJoints.a[0], dummy.currentJoints.a[1],
dummy.currentJoints.a[2], dummy.currentJoints.a[3],
dummy.currentJoints.a[4], dummy.currentJoints.a[5]);
} else if (s.find("GETLPOS") != std::string::npos)
{
dummy.UpdateJointPose6D();
Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
dummy.currentPose6D.X, dummy.currentPose6D.Y,
dummy.currentPose6D.Z, dummy.currentPose6D.A,
dummy.currentPose6D.B, dummy.currentPose6D.C);
} else if (s.find("CMDMODE") != std::string::npos)
{
uint32_t mode;
sscanf(_cmd, "#CMDMODE %lu", &mode);
dummy.SetCommandMode(mode);
Respond(_responseChannel, "Set command mode to [%lu]", mode);
} else
Respond(_responseChannel, "ok");
} else if (_cmd[0] == '>' || _cmd[0] == '@')
{
uint32_t freeSize = dummy.commandHandler.Push(_cmd);
Respond(_responseChannel, "%d", freeSize);
}
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
}
void OnUart4AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
{
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
if (_cmd[0] == '!' || !dummy.IsEnabled())
{
std::string s(_cmd);
if (s.find("STOP") != std::string::npos)
{
dummy.commandHandler.EmergencyStop();
Respond(_responseChannel, "Stopped ok");
} else if (s.find("START") != std::string::npos)
{
dummy.SetEnable(true);
Respond(_responseChannel, "Started ok");
} else if (s.find("DISABLE") != std::string::npos)
{
dummy.SetEnable(false);
Respond(_responseChannel, "Disabled ok");
}
} else if (_cmd[0] == '#')
{
std::string s(_cmd);
if (s.find("GETJPOS") != std::string::npos)
{
Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
dummy.currentJoints.a[0], dummy.currentJoints.a[1],
dummy.currentJoints.a[2], dummy.currentJoints.a[3],
dummy.currentJoints.a[4], dummy.currentJoints.a[5]);
} else if (s.find("GETLPOS") != std::string::npos)
{
dummy.UpdateJointPose6D();
Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
dummy.currentPose6D.X, dummy.currentPose6D.Y,
dummy.currentPose6D.Z, dummy.currentPose6D.A,
dummy.currentPose6D.B, dummy.currentPose6D.C);
} else if (s.find("CMDMODE") != std::string::npos)
{
uint32_t mode;
sscanf(_cmd, "#CMDMODE %lu", &mode);
dummy.SetCommandMode(mode);
Respond(_responseChannel, "Set command mode to [%lu]", mode);
} else
Respond(_responseChannel, "ok");
} else if (_cmd[0] == '>' || _cmd[0] == '@')
{
uint32_t freeSize = dummy.commandHandler.Push(_cmd);
Respond(_responseChannel, "%d", freeSize);
}
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
}
void OnUart5AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
{
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
}

View File

@ -1,44 +1,30 @@
#include "common_inc.h"
/*----------------- 1.Add Your Extern Variables Here (Optional) ------------------*/
extern DummyRobot dummy;
class HelperFunctions
{
public:
/*--------------- 2.Add Your Helper Functions Helper Here (optional) ----------------*/
int32_t TestFunction(int32_t delta)
{
static int cnt = 0;
return cnt += delta;
}
void SaveConfigurationHelper()
{}
void EraseConfigurationHelper()
{}
float GetTemperatureHelper()
{ return AdcGetChipTemperature(); }
void SystemResetHelper()
{ NVIC_SystemReset(); }
} staticFunctions;
// Define options that intractable with "reftool".
static inline auto MakeObjTree()
{
/*--------------- 3.Add Your Protocol Variables & Functions Here ----------------*/
return make_protocol_member_list(
// Add Read-Only Variables
make_protocol_ro_property("serial_number", &serialNumber),
make_protocol_object("robot", dummy.MakeProtocolDefinitions())
);
}
COMMIT_PROTOCOL
#include "common_inc.h"
/*----------------- 1.Add Your Extern Variables Here (Optional) ------------------*/
extern DummyRobot dummy;
class HelperFunctions
{
public:
/*--------------- 2.Add Your Helper Functions Helper Here (optional) ----------------*/
float GetTemperatureHelper()
{ return AdcGetChipTemperature(); }
} staticFunctions;
// Define options that intractable with "reftool".
static inline auto MakeObjTree()
{
/*--------------- 3.Add Your Protocol Variables & Functions Here ----------------*/
return make_protocol_member_list(
// Add Read-Only Variables
make_protocol_ro_property("serial_number", &serialNumber),
make_protocol_function("get_temperature", staticFunctions, &HelperFunctions::GetTemperatureHelper),
make_protocol_object("robot", dummy.MakeProtocolDefinitions())
);
}
COMMIT_PROTOCOL

View File

@ -1,2 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<?xml version="1.0" encoding="UTF-8"?>
<module classpath="CMake" type="CPP_MODULE" version="4" />

View File

@ -1,8 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/Ctrl-Step-fw.iml" filepath="$PROJECT_DIR$/.idea/Ctrl-Step-fw.iml" />
</modules>
</component>
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/Ctrl-Step-fw.iml" filepath="$PROJECT_DIR$/.idea/Ctrl-Step-fw.iml" />
</modules>
</component>
</project>