mirror of
https://gitee.com/peng_zhihui/Dummy-Robot
synced 2025-09-26 18:09:10 +08:00
[Fw] Update REF protocols.
This commit is contained in:
parent
f0e2c8da3a
commit
fe1007c668
2
2.Firmware/Core-STM32F4-fw/.idea/REF-STM32F4.iml
generated
2
2.Firmware/Core-STM32F4-fw/.idea/REF-STM32F4.iml
generated
@ -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" />
|
14
2.Firmware/Core-STM32F4-fw/.idea/modules.xml
generated
14
2.Firmware/Core-STM32F4-fw/.idea/modules.xml
generated
@ -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>
|
@ -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;
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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}")
|
||||
|
@ -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}")
|
||||
|
@ -1,175 +1,175 @@
|
||||
/* USER CODE BEGIN Header */
|
||||
/**
|
||||
******************************************************************************
|
||||
* File Name : freertos.c
|
||||
* Description : Code for freertos applications
|
||||
******************************************************************************
|
||||
* @attention
|
||||
*
|
||||
* <h2><center>© 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>© 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
@ -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);
|
||||
}
|
||||
|
@ -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
|
||||
|
File diff suppressed because it is too large
Load Diff
@ -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
@ -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
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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 ↑ -----------------------------*/
|
||||
}
|
@ -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
|
||||
|
@ -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" />
|
@ -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>
|
Loading…
Reference in New Issue
Block a user