mirror of
https://gitee.com/peng_zhihui/Dummy-Robot
synced 2025-09-27 02:09:12 +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" />
|
<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"?>
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
<project version="4">
|
<project version="4">
|
||||||
<component name="ProjectModuleManager">
|
<component name="ProjectModuleManager">
|
||||||
<modules>
|
<modules>
|
||||||
<module fileurl="file://$PROJECT_DIR$/.idea/REF-STM32F4.iml" filepath="$PROJECT_DIR$/.idea/REF-STM32F4.iml" />
|
<module fileurl="file://$PROJECT_DIR$/.idea/REF-STM32F4.iml" filepath="$PROJECT_DIR$/.idea/REF-STM32F4.iml" />
|
||||||
</modules>
|
</modules>
|
||||||
</component>
|
</component>
|
||||||
</project>
|
</project>
|
@ -1,95 +1,89 @@
|
|||||||
|
|
||||||
/* Includes ------------------------------------------------------------------*/
|
/* Includes ------------------------------------------------------------------*/
|
||||||
|
|
||||||
#include "communication.hpp"
|
#include "communication.hpp"
|
||||||
#include "common_inc.h"
|
#include "common_inc.h"
|
||||||
|
|
||||||
/* Private defines -----------------------------------------------------------*/
|
/* Private defines -----------------------------------------------------------*/
|
||||||
/* Private macros ------------------------------------------------------------*/
|
/* Private macros ------------------------------------------------------------*/
|
||||||
/* Private typedef -----------------------------------------------------------*/
|
/* Private typedef -----------------------------------------------------------*/
|
||||||
/* Global constant data ------------------------------------------------------*/
|
/* Global constant data ------------------------------------------------------*/
|
||||||
/* Global variables ----------------------------------------------------------*/
|
/* Global variables ----------------------------------------------------------*/
|
||||||
/* Private constant data -----------------------------------------------------*/
|
/* Private constant data -----------------------------------------------------*/
|
||||||
/* Private variables ---------------------------------------------------------*/
|
/* Private variables ---------------------------------------------------------*/
|
||||||
volatile bool endpointListValid = false;
|
volatile bool endpointListValid = false;
|
||||||
/* Private function prototypes -----------------------------------------------*/
|
/* Private function prototypes -----------------------------------------------*/
|
||||||
/* Function implementations --------------------------------------------------*/
|
/* Function implementations --------------------------------------------------*/
|
||||||
// @brief Sends a line on the specified output.
|
// @brief Sends a line on the specified output.
|
||||||
|
|
||||||
osThreadId_t commTaskHandle;
|
osThreadId_t commTaskHandle;
|
||||||
const osThreadAttr_t commTask_attributes = {
|
const osThreadAttr_t commTask_attributes = {
|
||||||
.name = "commTask",
|
.name = "commTask",
|
||||||
.stack_size = 10000 * 4,
|
.stack_size = 45000,
|
||||||
.priority = (osPriority_t) osPriorityNormal,
|
.priority = (osPriority_t) osPriorityNormal,
|
||||||
};
|
};
|
||||||
|
|
||||||
void InitCommunication(void)
|
void InitCommunication(void)
|
||||||
{
|
{
|
||||||
printf("\r\nHello, REF v%.1f Started!\r\n", CONFIG_FW_VERSION);
|
// Start command handling thread
|
||||||
|
commTaskHandle = osThreadNew(CommunicationTask, nullptr, &commTask_attributes);
|
||||||
// Start command handling thread
|
|
||||||
commTaskHandle = osThreadNew(CommunicationTask, NULL, &commTask_attributes);
|
while (!endpointListValid)
|
||||||
|
osDelay(1);
|
||||||
while (!endpointListValid)
|
}
|
||||||
osDelay(1);
|
|
||||||
}
|
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
|
||||||
|
osThreadId_t usbIrqTaskHandle;
|
||||||
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
|
|
||||||
osThreadId_t usbIrqTaskHandle;
|
void UsbDeferredInterruptTask(void* ctx)
|
||||||
|
{
|
||||||
void UsbDeferredInterruptTask(void* ctx)
|
(void) ctx; // unused parameter
|
||||||
{
|
|
||||||
(void) ctx; // unused parameter
|
for (;;)
|
||||||
|
{
|
||||||
for (;;)
|
// Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
|
||||||
{
|
osStatus semaphore_status = osSemaphoreAcquire(sem_usb_irq, osWaitForever);
|
||||||
// Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
|
if (semaphore_status == osOK)
|
||||||
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);
|
||||||
// We have a new incoming USB transmission: handle it
|
// Let the irq (OTG_FS_IRQHandler) fire again.
|
||||||
HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
|
HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
|
||||||
// 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
|
||||||
// Thread to handle deffered processing of USB interrupt, and
|
void CommunicationTask(void* ctx)
|
||||||
// read commands out of the UART DMA circular buffer
|
{
|
||||||
void CommunicationTask(void* ctx)
|
(void) ctx; // unused parameter
|
||||||
{
|
|
||||||
(void) ctx; // unused parameter
|
CommitProtocol();
|
||||||
|
|
||||||
CommitProtocol();
|
// Allow main init to continue
|
||||||
|
endpointListValid = true;
|
||||||
// Allow main init to continue
|
|
||||||
endpointListValid = true;
|
StartUartServer();
|
||||||
|
StartUsbServer();
|
||||||
StartUartServer();
|
StartCanServer(CAN1);
|
||||||
StartUsbServer();
|
StartCanServer(CAN2);
|
||||||
StartCanServer(CAN1);
|
|
||||||
StartCanServer(CAN2);
|
for (;;)
|
||||||
|
{
|
||||||
for (;;)
|
osDelay(1000); // nothing to do
|
||||||
{
|
}
|
||||||
osDelay(1000); // nothing to do
|
}
|
||||||
}
|
|
||||||
}
|
extern "C" {
|
||||||
|
int _write(int file, const char* data, int len);
|
||||||
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)
|
||||||
// @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);
|
||||||
#ifdef DEBUG_VIA_USB_SERIAL
|
|
||||||
usbStreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
|
return len;
|
||||||
uart4StreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
|
}
|
||||||
#endif
|
|
||||||
|
|
||||||
uart5StreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
|
|
||||||
|
|
||||||
return len;
|
|
||||||
}
|
|
||||||
|
@ -1,252 +1,250 @@
|
|||||||
/*
|
/*
|
||||||
*
|
*
|
||||||
* Zero-config node ID negotiation
|
* Zero-config node ID negotiation
|
||||||
* -------------------------------
|
* -------------------------------
|
||||||
*
|
*
|
||||||
* A heartbeat message is a message with a 8 byte unique serial number as payload.
|
* 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.
|
* A regular message is any message that is not a heartbeat message.
|
||||||
*
|
*
|
||||||
* All nodes MUST obey these four rules:
|
* All nodes MUST obey these four rules:
|
||||||
*
|
*
|
||||||
* a) At a given point in time, a node MUST consider a node ID taken (by others)
|
* a) At a given point in time, a node MUST consider a node ID taken (by others)
|
||||||
* if any of the following is true:
|
* if any of the following is true:
|
||||||
* - the node received a (not self-emitted) heartbeat message with that node ID
|
* - the node received a (not self-emitted) heartbeat message with that node ID
|
||||||
* within the last second
|
* within the last second
|
||||||
* - the node attempted and failed at sending a heartbeat message with that
|
* - 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)
|
* 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
|
* 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
|
* if, within the last second, it did not succeed in sending a heartbeat
|
||||||
* message with that node ID.
|
* message with that node ID.
|
||||||
*
|
*
|
||||||
* c) At a given point in time, a node MUST NOT send any heartbeat message with
|
* c) At a given point in time, a node MUST NOT send any heartbeat message with
|
||||||
* a node ID that is taken.
|
* a node ID that is taken.
|
||||||
*
|
*
|
||||||
* d) At a given point in time, a node MUST NOT send any regular message with
|
* d) At a given point in time, a node MUST NOT send any regular message with
|
||||||
* a node ID that is not self-assigned.
|
* a node ID that is not self-assigned.
|
||||||
*
|
*
|
||||||
* Hardware allocation
|
* Hardware allocation
|
||||||
* -------------------
|
* -------------------
|
||||||
* RX FIFO0:
|
* RX FIFO0:
|
||||||
* - filter bank 0: heartbeat messages
|
* - filter bank 0: heartbeat messages
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "common_inc.h"
|
#include "common_inc.h"
|
||||||
#include <stm32f4xx_hal.h>
|
#include <stm32f4xx_hal.h>
|
||||||
#include <cmsis_os.h>
|
#include <cmsis_os.h>
|
||||||
|
|
||||||
// defined in can.c
|
// defined in can.c
|
||||||
extern CAN_HandleTypeDef hcan1;
|
extern CAN_HandleTypeDef hcan1;
|
||||||
extern CAN_HandleTypeDef hcan2;
|
extern CAN_HandleTypeDef hcan2;
|
||||||
|
|
||||||
CAN_context can1Ctx;
|
CAN_context can1Ctx;
|
||||||
CAN_context can2Ctx;
|
CAN_context can2Ctx;
|
||||||
static CAN_context* ctxs = nullptr;
|
static CAN_context* ctxs = nullptr;
|
||||||
static CAN_RxHeaderTypeDef headerRx;
|
static CAN_RxHeaderTypeDef headerRx;
|
||||||
static uint8_t data[8];
|
static uint8_t data[8];
|
||||||
|
|
||||||
|
|
||||||
struct CAN_context* get_can_ctx(CAN_HandleTypeDef* hcan)
|
struct CAN_context* get_can_ctx(CAN_HandleTypeDef* hcan)
|
||||||
{
|
{
|
||||||
if (hcan->Instance == CAN1)
|
if (hcan->Instance == CAN1)
|
||||||
return &can1Ctx;
|
return &can1Ctx;
|
||||||
else if (hcan->Instance == CAN2)
|
else if (hcan->Instance == CAN2)
|
||||||
return &can2Ctx;
|
return &can2Ctx;
|
||||||
else
|
else
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool StartCanServer(CAN_TypeDef* hcan)
|
bool StartCanServer(CAN_TypeDef* hcan)
|
||||||
{
|
{
|
||||||
if (hcan == CAN1)
|
if (hcan == CAN1)
|
||||||
{
|
{
|
||||||
ctxs = &can1Ctx;
|
ctxs = &can1Ctx;
|
||||||
ctxs->handle = &hcan1;
|
ctxs->handle = &hcan1;
|
||||||
} else if (hcan == CAN2)
|
} else if (hcan == CAN2)
|
||||||
{
|
{
|
||||||
ctxs = &can2Ctx;
|
ctxs = &can2Ctx;
|
||||||
ctxs->handle = &hcan2;
|
ctxs->handle = &hcan2;
|
||||||
} else
|
} else
|
||||||
return false; // fail if none of the above checks matched
|
return false; // fail if none of the above checks matched
|
||||||
|
|
||||||
HAL_StatusTypeDef status;
|
HAL_StatusTypeDef status;
|
||||||
|
|
||||||
ctxs->node_id = 0;
|
ctxs->node_id = 0;
|
||||||
ctxs->serial_number = serialNumber;
|
ctxs->serial_number = serialNumber;
|
||||||
osSemaphoreDef(sem_send_heartbeat);
|
osSemaphoreDef(sem_send_heartbeat);
|
||||||
ctxs->sem_send_heartbeat = osSemaphoreNew(1, 0, osSemaphore(sem_send_heartbeat));
|
ctxs->sem_send_heartbeat = osSemaphoreNew(1, 0, osSemaphore(sem_send_heartbeat));
|
||||||
|
|
||||||
//// Set up filter
|
//// Set up filter
|
||||||
CAN_FilterTypeDef sFilterConfig = {
|
CAN_FilterTypeDef sFilterConfig = {
|
||||||
.FilterIdHigh = 0x0000,
|
.FilterIdHigh = 0x0000,
|
||||||
.FilterIdLow = 0x0000,
|
.FilterIdLow = 0x0000,
|
||||||
.FilterMaskIdHigh = 0x0000,
|
.FilterMaskIdHigh = 0x0000,
|
||||||
.FilterMaskIdLow = 0x0000,
|
.FilterMaskIdLow = 0x0000,
|
||||||
.FilterFIFOAssignment = CAN_RX_FIFO0,
|
.FilterFIFOAssignment = CAN_RX_FIFO0,
|
||||||
.FilterBank = 0,
|
.FilterBank = 0,
|
||||||
.FilterMode = CAN_FILTERMODE_IDMASK,
|
.FilterMode = CAN_FILTERMODE_IDMASK,
|
||||||
.FilterScale = CAN_FILTERSCALE_16BIT, // two 16-bit filters
|
.FilterScale = CAN_FILTERSCALE_16BIT, // two 16-bit filters
|
||||||
.FilterActivation = ENABLE,
|
.FilterActivation = ENABLE,
|
||||||
.SlaveStartFilterBank = 0
|
.SlaveStartFilterBank = 0
|
||||||
};
|
};
|
||||||
status = HAL_CAN_ConfigFilter(ctxs->handle, &sFilterConfig);
|
status = HAL_CAN_ConfigFilter(ctxs->handle, &sFilterConfig);
|
||||||
if (status != HAL_OK)
|
if (status != HAL_OK)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
status = HAL_CAN_Start(ctxs->handle);
|
status = HAL_CAN_Start(ctxs->handle);
|
||||||
if (status != HAL_OK)
|
if (status != HAL_OK)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
status = HAL_CAN_ActivateNotification(ctxs->handle,
|
status = HAL_CAN_ActivateNotification(ctxs->handle,
|
||||||
CAN_IT_TX_MAILBOX_EMPTY |
|
CAN_IT_TX_MAILBOX_EMPTY |
|
||||||
CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING |
|
CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING |
|
||||||
/* we probably only want this */
|
/* we probably only want this */
|
||||||
CAN_IT_RX_FIFO0_FULL | CAN_IT_RX_FIFO1_FULL |
|
CAN_IT_RX_FIFO0_FULL | CAN_IT_RX_FIFO1_FULL |
|
||||||
CAN_IT_RX_FIFO0_OVERRUN | CAN_IT_RX_FIFO1_OVERRUN |
|
CAN_IT_RX_FIFO0_OVERRUN | CAN_IT_RX_FIFO1_OVERRUN |
|
||||||
CAN_IT_WAKEUP | CAN_IT_SLEEP_ACK |
|
CAN_IT_WAKEUP | CAN_IT_SLEEP_ACK |
|
||||||
CAN_IT_ERROR_WARNING | CAN_IT_ERROR_PASSIVE |
|
CAN_IT_ERROR_WARNING | CAN_IT_ERROR_PASSIVE |
|
||||||
CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE |
|
CAN_IT_BUSOFF | CAN_IT_LAST_ERROR_CODE |
|
||||||
CAN_IT_ERROR);
|
CAN_IT_ERROR);
|
||||||
if (status != HAL_OK)
|
if (status != HAL_OK)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void tx_complete_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx)
|
void tx_complete_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx)
|
||||||
{
|
{
|
||||||
// CAN_context* ctx = get_can_ctx(hcan);
|
// CAN_context* ctx = get_can_ctx(hcan);
|
||||||
// if (!ctx) return;
|
// if (!ctx) return;
|
||||||
// ctx->tx_msg_cnt++;
|
// ctx->tx_msg_cnt++;
|
||||||
|
|
||||||
if (hcan->Instance == CAN1)
|
if (hcan->Instance == CAN1)
|
||||||
osSemaphoreRelease(sem_can1_tx);
|
osSemaphoreRelease(sem_can1_tx);
|
||||||
else if (hcan->Instance == CAN2)
|
else if (hcan->Instance == CAN2)
|
||||||
osSemaphoreRelease(sem_can2_tx);
|
osSemaphoreRelease(sem_can2_tx);
|
||||||
}
|
}
|
||||||
|
|
||||||
void tx_aborted_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx)
|
void tx_aborted_callback(CAN_HandleTypeDef* hcan, uint8_t mailbox_idx)
|
||||||
{
|
{
|
||||||
if (!get_can_ctx(hcan))
|
if (!get_can_ctx(hcan))
|
||||||
return;
|
return;
|
||||||
get_can_ctx(hcan)->TxMailboxAbortCallbackCnt++;
|
get_can_ctx(hcan)->TxMailboxAbortCallbackCnt++;
|
||||||
}
|
}
|
||||||
|
|
||||||
void tx_error(CAN_context* ctx, uint8_t mailbox_idx)
|
void tx_error(CAN_context* ctx, uint8_t mailbox_idx)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef* hcan)
|
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef* hcan)
|
||||||
{ tx_complete_callback(hcan, 0); }
|
{ tx_complete_callback(hcan, 0); }
|
||||||
|
|
||||||
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef* hcan)
|
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef* hcan)
|
||||||
{ tx_complete_callback(hcan, 1); }
|
{ tx_complete_callback(hcan, 1); }
|
||||||
|
|
||||||
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef* hcan)
|
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef* hcan)
|
||||||
{ tx_complete_callback(hcan, 2); }
|
{ tx_complete_callback(hcan, 2); }
|
||||||
|
|
||||||
void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef* hcan)
|
void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef* hcan)
|
||||||
{ tx_aborted_callback(hcan, 0); }
|
{ tx_aborted_callback(hcan, 0); }
|
||||||
|
|
||||||
void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef* hcan)
|
void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef* hcan)
|
||||||
{ tx_aborted_callback(hcan, 1); }
|
{ tx_aborted_callback(hcan, 1); }
|
||||||
|
|
||||||
void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef* hcan)
|
void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef* hcan)
|
||||||
{ tx_aborted_callback(hcan, 2); }
|
{ tx_aborted_callback(hcan, 2); }
|
||||||
|
|
||||||
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan)
|
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* hcan)
|
||||||
{
|
{
|
||||||
CAN_context* ctx = get_can_ctx(hcan);
|
CAN_context* ctx = get_can_ctx(hcan);
|
||||||
if (!ctx) return;
|
if (!ctx) return;
|
||||||
ctx->received_msg_cnt++;
|
ctx->received_msg_cnt++;
|
||||||
|
|
||||||
HAL_StatusTypeDef status = HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &headerRx, data);
|
HAL_StatusTypeDef status = HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &headerRx, data);
|
||||||
if (status != HAL_OK)
|
if (status != HAL_OK)
|
||||||
{
|
{
|
||||||
ctx->unexpected_errors++;
|
ctx->unexpected_errors++;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
OnCanMessage(ctx, &headerRx, data);
|
OnCanMessage(ctx, &headerRx, data);
|
||||||
}
|
}
|
||||||
|
|
||||||
void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef* hcan)
|
void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef* hcan)
|
||||||
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo0FullCallbackCnt++; }
|
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo0FullCallbackCnt++; }
|
||||||
|
|
||||||
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef* hcan)
|
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef* hcan)
|
||||||
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1MsgPendingCallbackCnt++; }
|
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1MsgPendingCallbackCnt++; }
|
||||||
|
|
||||||
void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef* hcan)
|
void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef* hcan)
|
||||||
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1FullCallbackCnt++; }
|
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->RxFifo1FullCallbackCnt++; }
|
||||||
|
|
||||||
void HAL_CAN_SleepCallback(CAN_HandleTypeDef* hcan)
|
void HAL_CAN_SleepCallback(CAN_HandleTypeDef* hcan)
|
||||||
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->SleepCallbackCnt++; }
|
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->SleepCallbackCnt++; }
|
||||||
|
|
||||||
void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef* hcan)
|
void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef* hcan)
|
||||||
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->WakeUpFromRxMsgCallbackCnt++; }
|
{ if (get_can_ctx(hcan)) get_can_ctx(hcan)->WakeUpFromRxMsgCallbackCnt++; }
|
||||||
|
|
||||||
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef* hcan)
|
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef* hcan)
|
||||||
{
|
{
|
||||||
//__asm volatile ("bkpt");
|
//__asm volatile ("bkpt");
|
||||||
CAN_context* ctx = get_can_ctx(hcan);
|
CAN_context* ctx = get_can_ctx(hcan);
|
||||||
if (!ctx) return;
|
if (!ctx) return;
|
||||||
volatile uint32_t original_error = hcan->ErrorCode;
|
volatile uint32_t original_error = hcan->ErrorCode;
|
||||||
(void) original_error;
|
(void) original_error;
|
||||||
|
|
||||||
// handle transmit errors in all three mailboxes
|
// handle transmit errors in all three mailboxes
|
||||||
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST0)
|
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST0)
|
||||||
{
|
{
|
||||||
SET_BIT(hcan->Instance->sTxMailBox[0].TIR, CAN_TI0R_TXRQ);
|
SET_BIT(hcan->Instance->sTxMailBox[0].TIR, CAN_TI0R_TXRQ);
|
||||||
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST0;
|
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST0;
|
||||||
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR0)
|
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR0)
|
||||||
{
|
{
|
||||||
tx_error(ctx, 0);
|
tx_error(ctx, 0);
|
||||||
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
|
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
|
||||||
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
|
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
|
||||||
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR0;
|
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR0;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST1)
|
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST1)
|
||||||
{
|
{
|
||||||
SET_BIT(hcan->Instance->sTxMailBox[1].TIR, CAN_TI1R_TXRQ);
|
SET_BIT(hcan->Instance->sTxMailBox[1].TIR, CAN_TI1R_TXRQ);
|
||||||
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST1;
|
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST1;
|
||||||
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR1)
|
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR1)
|
||||||
{
|
{
|
||||||
tx_error(ctx, 1);
|
tx_error(ctx, 1);
|
||||||
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
|
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
|
||||||
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
|
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
|
||||||
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR1;
|
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST2)
|
if (hcan->ErrorCode & HAL_CAN_ERROR_TX_ALST2)
|
||||||
{
|
{
|
||||||
SET_BIT(hcan->Instance->sTxMailBox[2].TIR, CAN_TI2R_TXRQ);
|
SET_BIT(hcan->Instance->sTxMailBox[2].TIR, CAN_TI2R_TXRQ);
|
||||||
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST2;
|
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_ALST2;
|
||||||
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR2)
|
} else if (hcan->ErrorCode & HAL_CAN_ERROR_TX_TERR2)
|
||||||
{
|
{
|
||||||
tx_error(ctx, 2);
|
tx_error(ctx, 2);
|
||||||
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
|
hcan->ErrorCode &= ~HAL_CAN_ERROR_EWG;
|
||||||
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
|
hcan->ErrorCode &= ~HAL_CAN_ERROR_ACK;
|
||||||
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR2;
|
hcan->ErrorCode &= ~HAL_CAN_ERROR_TX_TERR2;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (hcan->ErrorCode)
|
if (hcan->ErrorCode)
|
||||||
ctx->unexpected_errors++;
|
ctx->unexpected_errors++;
|
||||||
}
|
}
|
||||||
|
|
||||||
void CanSendMessage(CAN_context* canCtx, uint8_t* txData, CAN_TxHeaderTypeDef* txHeader)
|
void CanSendMessage(CAN_context* canCtx, uint8_t* txData, CAN_TxHeaderTypeDef* txHeader)
|
||||||
{
|
{
|
||||||
osStatus semaphore_status;
|
osStatus semaphore_status;
|
||||||
if (canCtx->handle->Instance == CAN1)
|
if (canCtx->handle->Instance == CAN1)
|
||||||
semaphore_status = osSemaphoreAcquire(sem_can1_tx, osWaitForever);
|
semaphore_status = osSemaphoreAcquire(sem_can1_tx, osWaitForever);
|
||||||
else if (canCtx->handle->Instance == CAN2)
|
else if (canCtx->handle->Instance == CAN2)
|
||||||
semaphore_status = osSemaphoreAcquire(sem_can2_tx, osWaitForever);
|
semaphore_status = osSemaphoreAcquire(sem_can2_tx, osWaitForever);
|
||||||
else
|
else
|
||||||
return;
|
return;
|
||||||
|
|
||||||
if (semaphore_status == osOK)
|
if (semaphore_status == osOK)
|
||||||
{
|
HAL_CAN_AddTxMessage(canCtx->handle, txHeader, txData, &canCtx->last_heartbeat_mailbox);
|
||||||
HAL_CAN_AddTxMessage(canCtx->handle, txHeader, txData, &canCtx->last_heartbeat_mailbox);
|
}
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
@ -1,206 +1,206 @@
|
|||||||
#include "common_inc.h"
|
#include "common_inc.h"
|
||||||
#include "interface_uart.hpp"
|
#include "interface_uart.hpp"
|
||||||
#include "ascii_processor.hpp"
|
#include "ascii_processor.hpp"
|
||||||
#include "fibre/protocol.hpp"
|
#include "fibre/protocol.hpp"
|
||||||
#include "usart.h"
|
#include "usart.h"
|
||||||
|
|
||||||
#define UART_TX_BUFFER_SIZE 64
|
#define UART_TX_BUFFER_SIZE 64
|
||||||
#define UART_RX_BUFFER_SIZE 64
|
#define UART_RX_BUFFER_SIZE 64
|
||||||
|
|
||||||
// DMA open loop continous circular buffer
|
// DMA open loop continous circular buffer
|
||||||
// 1ms delay periodic, chase DMA ptr around
|
// 1ms delay periodic, chase DMA ptr around
|
||||||
static uint8_t dma_rx_buffer[2][UART_RX_BUFFER_SIZE];
|
static uint8_t dma_rx_buffer[2][UART_RX_BUFFER_SIZE];
|
||||||
static uint32_t dma_last_rcv_idx[2];
|
static uint32_t dma_last_rcv_idx[2];
|
||||||
|
|
||||||
// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable
|
// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable
|
||||||
// static thread_local uint32_t deadline_ms = 0;
|
// static thread_local uint32_t deadline_ms = 0;
|
||||||
|
|
||||||
osThreadId_t uartServerTaskHandle;
|
osThreadId_t uartServerTaskHandle;
|
||||||
|
|
||||||
|
|
||||||
class UART4Sender : public StreamSink
|
class UART4Sender : public StreamSink
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
UART4Sender()
|
UART4Sender()
|
||||||
{
|
{
|
||||||
channelType = CHANNEL_TYPE_UART4;
|
channelType = CHANNEL_TYPE_UART4;
|
||||||
}
|
}
|
||||||
|
|
||||||
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override
|
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override
|
||||||
{
|
{
|
||||||
// Loop to ensure all bytes get sent
|
// Loop to ensure all bytes get sent
|
||||||
while (length)
|
while (length)
|
||||||
{
|
{
|
||||||
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
|
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
|
||||||
// wait for USB interface to become ready
|
// wait for USB interface to become ready
|
||||||
// TODO: implement ring buffer to get a more continuous stream of data
|
// TODO: implement ring buffer to get a more continuous stream of data
|
||||||
// if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
|
// if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
|
||||||
if (osSemaphoreAcquire(sem_uart4_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
|
if (osSemaphoreAcquire(sem_uart4_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
|
||||||
return -1;
|
return -1;
|
||||||
// transmit chunk
|
// transmit chunk
|
||||||
memcpy(tx_buf_, buffer, chunk);
|
memcpy(tx_buf_, buffer, chunk);
|
||||||
if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK)
|
if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK)
|
||||||
return -1;
|
return -1;
|
||||||
buffer += chunk;
|
buffer += chunk;
|
||||||
length -= chunk;
|
length -= chunk;
|
||||||
if (processed_bytes)
|
if (processed_bytes)
|
||||||
*processed_bytes += chunk;
|
*processed_bytes += chunk;
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
size_t get_free_space() override
|
size_t get_free_space() override
|
||||||
{ return SIZE_MAX; }
|
{ return SIZE_MAX; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
|
uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
|
||||||
} uart4_stream_output;
|
} uart4_stream_output;
|
||||||
|
|
||||||
class UART5Sender : public StreamSink
|
class UART5Sender : public StreamSink
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
UART5Sender()
|
UART5Sender()
|
||||||
{
|
{
|
||||||
channelType = CHANNEL_TYPE_UART5;
|
channelType = CHANNEL_TYPE_UART5;
|
||||||
}
|
}
|
||||||
|
|
||||||
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override
|
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override
|
||||||
{
|
{
|
||||||
// Loop to ensure all bytes get sent
|
// Loop to ensure all bytes get sent
|
||||||
while (length)
|
while (length)
|
||||||
{
|
{
|
||||||
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
|
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
|
||||||
// wait for USB interface to become ready
|
// wait for USB interface to become ready
|
||||||
// TODO: implement ring buffer to get a more continuous stream of data
|
// TODO: implement ring buffer to get a more continuous stream of data
|
||||||
// if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
|
// if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
|
||||||
if (osSemaphoreAcquire(sem_uart5_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
|
if (osSemaphoreAcquire(sem_uart5_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
|
||||||
return -1;
|
return -1;
|
||||||
// transmit chunk
|
// transmit chunk
|
||||||
memcpy(tx_buf_, buffer, chunk);
|
memcpy(tx_buf_, buffer, chunk);
|
||||||
if (HAL_UART_Transmit_DMA(&huart5, tx_buf_, chunk) != HAL_OK)
|
if (HAL_UART_Transmit_DMA(&huart5, tx_buf_, chunk) != HAL_OK)
|
||||||
return -1;
|
return -1;
|
||||||
buffer += chunk;
|
buffer += chunk;
|
||||||
length -= chunk;
|
length -= chunk;
|
||||||
if (processed_bytes)
|
if (processed_bytes)
|
||||||
*processed_bytes += chunk;
|
*processed_bytes += chunk;
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
size_t get_free_space() override
|
size_t get_free_space() override
|
||||||
{ return SIZE_MAX; }
|
{ return SIZE_MAX; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
|
uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
|
||||||
} uart5_stream_output;
|
} uart5_stream_output;
|
||||||
|
|
||||||
StreamSink* uart4StreamOutputPtr = &uart4_stream_output;
|
StreamSink* uart4StreamOutputPtr = &uart4_stream_output;
|
||||||
StreamBasedPacketSink uart4_packet_output(uart4_stream_output);
|
StreamBasedPacketSink uart4_packet_output(uart4_stream_output);
|
||||||
BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output);
|
BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output);
|
||||||
StreamToPacketSegmenter uart4_stream_input(uart4_channel);
|
StreamToPacketSegmenter uart4_stream_input(uart4_channel);
|
||||||
|
|
||||||
StreamSink* uart5StreamOutputPtr = &uart5_stream_output;
|
StreamSink* uart5StreamOutputPtr = &uart5_stream_output;
|
||||||
StreamBasedPacketSink uart5_packet_output(uart5_stream_output);
|
StreamBasedPacketSink uart5_packet_output(uart5_stream_output);
|
||||||
BidirectionalPacketBasedChannel uart5_channel(uart5_packet_output);
|
BidirectionalPacketBasedChannel uart5_channel(uart5_packet_output);
|
||||||
StreamToPacketSegmenter uart5_stream_input(uart5_channel);
|
StreamToPacketSegmenter uart5_stream_input(uart5_channel);
|
||||||
|
|
||||||
static void UartServerTask(void* ctx)
|
static void UartServerTask(void* ctx)
|
||||||
{
|
{
|
||||||
(void) ctx;
|
(void) ctx;
|
||||||
|
|
||||||
for (;;)
|
for (;;)
|
||||||
{
|
{
|
||||||
// Check for UART errors and restart recieve DMA transfer if required
|
// Check for UART errors and restart recieve DMA transfer if required
|
||||||
if (huart4.ErrorCode != HAL_UART_ERROR_NONE)
|
if (huart4.ErrorCode != HAL_UART_ERROR_NONE)
|
||||||
{
|
{
|
||||||
HAL_UART_AbortReceive(&huart4);
|
HAL_UART_AbortReceive(&huart4);
|
||||||
HAL_UART_Receive_DMA(&huart4, dma_rx_buffer[0], sizeof(dma_rx_buffer[0]));
|
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
|
// Fetch the circular buffer "write pointer", where it would write next
|
||||||
uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
|
uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
|
||||||
|
|
||||||
// deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
|
// deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
|
||||||
// Process bytes in one or two chunks (two in case there was a wrap)
|
// Process bytes in one or two chunks (two in case there was a wrap)
|
||||||
if (new_rcv_idx < dma_last_rcv_idx[0])
|
if (new_rcv_idx < dma_last_rcv_idx[0])
|
||||||
{
|
{
|
||||||
uart4_stream_input.process_bytes(dma_rx_buffer[0] + 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],
|
UART_RX_BUFFER_SIZE - dma_last_rcv_idx[0],
|
||||||
nullptr); // TODO: use process_all
|
nullptr); // TODO: use process_all
|
||||||
ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0],
|
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);
|
UART_RX_BUFFER_SIZE - dma_last_rcv_idx[0], uart4_stream_output);
|
||||||
dma_last_rcv_idx[0] = 0;
|
dma_last_rcv_idx[0] = 0;
|
||||||
}
|
}
|
||||||
if (new_rcv_idx > dma_last_rcv_idx[0])
|
if (new_rcv_idx > dma_last_rcv_idx[0])
|
||||||
{
|
{
|
||||||
uart4_stream_input.process_bytes(dma_rx_buffer[0] + 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],
|
new_rcv_idx - dma_last_rcv_idx[0],
|
||||||
nullptr); // TODO: use process_all
|
nullptr); // TODO: use process_all
|
||||||
ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0],
|
ASCII_protocol_parse_stream(dma_rx_buffer[0] + dma_last_rcv_idx[0],
|
||||||
new_rcv_idx - dma_last_rcv_idx[0], uart4_stream_output);
|
new_rcv_idx - dma_last_rcv_idx[0], uart4_stream_output);
|
||||||
dma_last_rcv_idx[0] = new_rcv_idx;
|
dma_last_rcv_idx[0] = new_rcv_idx;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// Check for UART errors and restart recieve DMA transfer if required
|
// Check for UART errors and restart recieve DMA transfer if required
|
||||||
if (huart5.ErrorCode != HAL_UART_ERROR_NONE)
|
if (huart5.ErrorCode != HAL_UART_ERROR_NONE)
|
||||||
{
|
{
|
||||||
HAL_UART_AbortReceive(&huart5);
|
HAL_UART_AbortReceive(&huart5);
|
||||||
HAL_UART_Receive_DMA(&huart5, dma_rx_buffer[1], sizeof(dma_rx_buffer[1]));
|
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
|
// Fetch the circular buffer "write pointer", where it would write next
|
||||||
new_rcv_idx = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR;
|
new_rcv_idx = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR;
|
||||||
|
|
||||||
// deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
|
// deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
|
||||||
// Process bytes in one or two chunks (two in case there was a wrap)
|
// Process bytes in one or two chunks (two in case there was a wrap)
|
||||||
if (new_rcv_idx < dma_last_rcv_idx[1])
|
if (new_rcv_idx < dma_last_rcv_idx[1])
|
||||||
{
|
{
|
||||||
uart4_stream_input.process_bytes(dma_rx_buffer[1] + 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],
|
UART_RX_BUFFER_SIZE - dma_last_rcv_idx[1],
|
||||||
nullptr); // TODO: use process_all
|
nullptr); // TODO: use process_all
|
||||||
ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1],
|
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);
|
UART_RX_BUFFER_SIZE - dma_last_rcv_idx[1], uart5_stream_output);
|
||||||
dma_last_rcv_idx[1] = 0;
|
dma_last_rcv_idx[1] = 0;
|
||||||
}
|
}
|
||||||
if (new_rcv_idx > dma_last_rcv_idx[1])
|
if (new_rcv_idx > dma_last_rcv_idx[1])
|
||||||
{
|
{
|
||||||
uart4_stream_input.process_bytes(dma_rx_buffer[1] + 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],
|
new_rcv_idx - dma_last_rcv_idx[1],
|
||||||
nullptr); // TODO: use process_all
|
nullptr); // TODO: use process_all
|
||||||
ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1],
|
ASCII_protocol_parse_stream(dma_rx_buffer[1] + dma_last_rcv_idx[1],
|
||||||
new_rcv_idx - dma_last_rcv_idx[1], uart5_stream_output);
|
new_rcv_idx - dma_last_rcv_idx[1], uart5_stream_output);
|
||||||
dma_last_rcv_idx[1] = new_rcv_idx;
|
dma_last_rcv_idx[1] = new_rcv_idx;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
osDelay(1);
|
osDelay(1);
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
const osThreadAttr_t uartServerTask_attributes = {
|
const osThreadAttr_t uartServerTask_attributes = {
|
||||||
.name = "UartServerTask",
|
.name = "UartServerTask",
|
||||||
.stack_size = 1000 * 4,
|
.stack_size = 2000,
|
||||||
.priority = (osPriority_t) osPriorityNormal,
|
.priority = (osPriority_t) osPriorityNormal,
|
||||||
};
|
};
|
||||||
|
|
||||||
void StartUartServer()
|
void StartUartServer()
|
||||||
{
|
{
|
||||||
// DMA is set up to recieve in a circular buffer forever.
|
// DMA is set up to receive in a circular buffer forever.
|
||||||
// We dont use interrupts to fetch the data, instead we periodically read
|
// 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
|
// 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]));
|
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;
|
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]));
|
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;
|
dma_last_rcv_idx[1] = UART_RX_BUFFER_SIZE - huart5.hdmarx->Instance->NDTR;
|
||||||
|
|
||||||
// Start UART communication thread
|
// Start UART communication thread
|
||||||
uartServerTaskHandle = osThreadNew(UartServerTask, nullptr, &uartServerTask_attributes);
|
uartServerTaskHandle = osThreadNew(UartServerTask, nullptr, &uartServerTask_attributes);
|
||||||
}
|
}
|
||||||
|
|
||||||
void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart)
|
void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart)
|
||||||
{
|
{
|
||||||
if (huart->Instance == UART4)
|
if (huart->Instance == UART4)
|
||||||
osSemaphoreRelease(sem_uart4_dma);
|
osSemaphoreRelease(sem_uart4_dma);
|
||||||
else if (huart->Instance == UART5)
|
else if (huart->Instance == UART5)
|
||||||
osSemaphoreRelease(sem_uart5_dma);
|
osSemaphoreRelease(sem_uart5_dma);
|
||||||
}
|
}
|
||||||
|
@ -1,190 +1,190 @@
|
|||||||
#include "common_inc.h"
|
#include "common_inc.h"
|
||||||
#include "ascii_processor.hpp"
|
#include "ascii_processor.hpp"
|
||||||
#include "usbd_cdc.h"
|
#include "usbd_cdc.h"
|
||||||
#include "usbd_cdc_if.h"
|
#include "usbd_cdc_if.h"
|
||||||
#include "usb_device.h"
|
#include "usb_device.h"
|
||||||
#include "interface_usb.hpp"
|
#include "interface_usb.hpp"
|
||||||
|
|
||||||
osThreadId_t usbServerTaskHandle;
|
osThreadId_t usbServerTaskHandle;
|
||||||
USBStats_t usb_stats_ = {0};
|
USBStats_t usb_stats_ = {0};
|
||||||
|
|
||||||
class USBSender : public PacketSink
|
class USBSender : public PacketSink
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
USBSender(uint8_t endpoint_pair, const osSemaphoreId &sem_usb_tx)
|
USBSender(uint8_t endpoint_pair, const osSemaphoreId &sem_usb_tx)
|
||||||
: endpoint_pair_(endpoint_pair), sem_usb_tx_(sem_usb_tx)
|
: endpoint_pair_(endpoint_pair), sem_usb_tx_(sem_usb_tx)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
int process_packet(const uint8_t *buffer, size_t length) override
|
int process_packet(const uint8_t *buffer, size_t length) override
|
||||||
{
|
{
|
||||||
// cannot send partial packets
|
// cannot send partial packets
|
||||||
if (length > USB_TX_DATA_SIZE)
|
if (length > USB_TX_DATA_SIZE)
|
||||||
return -1;
|
return -1;
|
||||||
// wait for USB interface to become ready
|
// wait for USB interface to become ready
|
||||||
if (osSemaphoreAcquire(sem_usb_tx_, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
|
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
|
// 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
|
// 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.
|
// TX buffer if this wait times out. The implication is that the channel is no longer lossless.
|
||||||
// TODO: handle endpoint reset properly
|
// TODO: handle endpoint reset properly
|
||||||
usb_stats_.tx_overrun_cnt++;
|
usb_stats_.tx_overrun_cnt++;
|
||||||
}
|
}
|
||||||
|
|
||||||
// transmit packet
|
// transmit packet
|
||||||
uint8_t status = CDC_Transmit_FS(const_cast<uint8_t *>(buffer), length, endpoint_pair_);
|
uint8_t status = CDC_Transmit_FS(const_cast<uint8_t *>(buffer), length, endpoint_pair_);
|
||||||
if (status != USBD_OK)
|
if (status != USBD_OK)
|
||||||
{
|
{
|
||||||
osSemaphoreRelease(sem_usb_tx_);
|
osSemaphoreRelease(sem_usb_tx_);
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
usb_stats_.tx_cnt++;
|
usb_stats_.tx_cnt++;
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
uint8_t endpoint_pair_;
|
uint8_t endpoint_pair_;
|
||||||
const osSemaphoreId &sem_usb_tx_;
|
const osSemaphoreId &sem_usb_tx_;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Note we could have independent semaphores here to allow concurrent transmission
|
// 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_cdc(CDC_OUT_EP, sem_usb_tx);
|
||||||
USBSender usb_packet_output_native(ODRIVE_OUT_EP, sem_usb_tx);
|
USBSender usb_packet_output_native(ODRIVE_OUT_EP, sem_usb_tx);
|
||||||
|
|
||||||
class TreatPacketSinkAsStreamSink : public StreamSink
|
class TreatPacketSinkAsStreamSink : public StreamSink
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
TreatPacketSinkAsStreamSink(PacketSink &output) : output_(output)
|
TreatPacketSinkAsStreamSink(PacketSink &output) : output_(output)
|
||||||
{
|
{
|
||||||
channelType = CHANNEL_TYPE_USB;
|
channelType = CHANNEL_TYPE_USB;
|
||||||
}
|
}
|
||||||
|
|
||||||
int process_bytes(const uint8_t *buffer, size_t length, size_t *processed_bytes)
|
int process_bytes(const uint8_t *buffer, size_t length, size_t *processed_bytes)
|
||||||
{
|
{
|
||||||
// Loop to ensure all bytes get sent
|
// Loop to ensure all bytes get sent
|
||||||
while (length)
|
while (length)
|
||||||
{
|
{
|
||||||
size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE;
|
size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE;
|
||||||
if (output_.process_packet(buffer, length) != 0)
|
if (output_.process_packet(buffer, length) != 0)
|
||||||
return -1;
|
return -1;
|
||||||
buffer += chunk;
|
buffer += chunk;
|
||||||
length -= chunk;
|
length -= chunk;
|
||||||
if (processed_bytes)
|
if (processed_bytes)
|
||||||
*processed_bytes += chunk;
|
*processed_bytes += chunk;
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
size_t get_free_space()
|
size_t get_free_space()
|
||||||
{ return SIZE_MAX; }
|
{ return SIZE_MAX; }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
PacketSink &output_;
|
PacketSink &output_;
|
||||||
} usb_stream_output(usb_packet_output_cdc);
|
} 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)
|
// This is used by the printf feature. Hence the above statics, and below seemingly random ptr (it's externed)
|
||||||
// TODO: less spaghetti code
|
// TODO: less spaghetti code
|
||||||
StreamSink *usbStreamOutputPtr = &usb_stream_output;
|
StreamSink *usbStreamOutputPtr = &usb_stream_output;
|
||||||
|
|
||||||
BidirectionalPacketBasedChannel usb_channel(usb_packet_output_native);
|
BidirectionalPacketBasedChannel usb_channel(usb_packet_output_native);
|
||||||
|
|
||||||
|
|
||||||
struct USBInterface
|
struct USBInterface
|
||||||
{
|
{
|
||||||
uint8_t *rx_buf = nullptr;
|
uint8_t *rx_buf = nullptr;
|
||||||
uint32_t rx_len = 0;
|
uint32_t rx_len = 0;
|
||||||
bool data_pending = false;
|
bool data_pending = false;
|
||||||
uint8_t out_ep;
|
uint8_t out_ep;
|
||||||
uint8_t in_ep;
|
uint8_t in_ep;
|
||||||
USBSender &usb_sender;
|
USBSender &usb_sender;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Note: statics make this less modular.
|
// 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
|
// Note: we use a single rx semaphore and loop over data_pending to allow a single pump loop thread
|
||||||
static USBInterface CDC_interface = {
|
static USBInterface CDC_interface = {
|
||||||
.rx_buf = nullptr,
|
.rx_buf = nullptr,
|
||||||
.rx_len = 0,
|
.rx_len = 0,
|
||||||
.data_pending = false,
|
.data_pending = false,
|
||||||
.out_ep = CDC_OUT_EP,
|
.out_ep = CDC_OUT_EP,
|
||||||
.in_ep = CDC_IN_EP,
|
.in_ep = CDC_IN_EP,
|
||||||
.usb_sender = usb_packet_output_cdc,
|
.usb_sender = usb_packet_output_cdc,
|
||||||
};
|
};
|
||||||
static USBInterface ODrive_interface = {
|
static USBInterface ODrive_interface = {
|
||||||
.rx_buf = nullptr,
|
.rx_buf = nullptr,
|
||||||
.rx_len = 0,
|
.rx_len = 0,
|
||||||
.data_pending = false,
|
.data_pending = false,
|
||||||
.out_ep = ODRIVE_OUT_EP,
|
.out_ep = ODRIVE_OUT_EP,
|
||||||
.in_ep = ODRIVE_IN_EP,
|
.in_ep = ODRIVE_IN_EP,
|
||||||
.usb_sender = usb_packet_output_native,
|
.usb_sender = usb_packet_output_native,
|
||||||
};
|
};
|
||||||
|
|
||||||
static void UsbServerTask(void *ctx)
|
static void UsbServerTask(void *ctx)
|
||||||
{
|
{
|
||||||
(void) ctx;
|
(void) ctx;
|
||||||
|
|
||||||
for (;;)
|
for (;;)
|
||||||
{
|
{
|
||||||
// const uint32_t usb_check_timeout = 1; // ms
|
// const uint32_t usb_check_timeout = 1; // ms
|
||||||
osStatus sem_stat = osSemaphoreAcquire(sem_usb_rx, osWaitForever);
|
osStatus sem_stat = osSemaphoreAcquire(sem_usb_rx, osWaitForever);
|
||||||
if (sem_stat == osOK)
|
if (sem_stat == osOK)
|
||||||
{
|
{
|
||||||
usb_stats_.rx_cnt++;
|
usb_stats_.rx_cnt++;
|
||||||
|
|
||||||
// CDC Interface
|
// CDC Interface
|
||||||
if (CDC_interface.data_pending)
|
if (CDC_interface.data_pending)
|
||||||
{
|
{
|
||||||
CDC_interface.data_pending = false;
|
CDC_interface.data_pending = false;
|
||||||
|
|
||||||
ASCII_protocol_parse_stream(CDC_interface.rx_buf, CDC_interface.rx_len, usb_stream_output);
|
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
|
USBD_CDC_ReceivePacket(&hUsbDeviceFS, CDC_interface.out_ep); // Allow next packet
|
||||||
}
|
}
|
||||||
|
|
||||||
// Native Interface
|
// Native Interface
|
||||||
if (ODrive_interface.data_pending)
|
if (ODrive_interface.data_pending)
|
||||||
{
|
{
|
||||||
ODrive_interface.data_pending = false;
|
ODrive_interface.data_pending = false;
|
||||||
usb_channel.process_packet(ODrive_interface.rx_buf, ODrive_interface.rx_len);
|
usb_channel.process_packet(ODrive_interface.rx_buf, ODrive_interface.rx_len);
|
||||||
USBD_CDC_ReceivePacket(&hUsbDeviceFS, ODrive_interface.out_ep); // Allow next packet
|
USBD_CDC_ReceivePacket(&hUsbDeviceFS, ODrive_interface.out_ep); // Allow next packet
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Called from CDC_Receive_FS callback function, this allows the communication
|
// Called from CDC_Receive_FS callback function, this allows the communication
|
||||||
// thread to handle the incoming data
|
// thread to handle the incoming data
|
||||||
void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair)
|
void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair)
|
||||||
{
|
{
|
||||||
USBInterface *usb_iface;
|
USBInterface *usb_iface;
|
||||||
if (endpoint_pair == CDC_interface.out_ep)
|
if (endpoint_pair == CDC_interface.out_ep)
|
||||||
{
|
{
|
||||||
usb_iface = &CDC_interface;
|
usb_iface = &CDC_interface;
|
||||||
} else if (endpoint_pair == ODrive_interface.out_ep)
|
} else if (endpoint_pair == ODrive_interface.out_ep)
|
||||||
{
|
{
|
||||||
usb_iface = &ODrive_interface;
|
usb_iface = &ODrive_interface;
|
||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// We don't allow the next USB packet until the previous one has been processed completely.
|
// 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.
|
// 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_buf = buf;
|
||||||
usb_iface->rx_len = len;
|
usb_iface->rx_len = len;
|
||||||
usb_iface->data_pending = true;
|
usb_iface->data_pending = true;
|
||||||
osSemaphoreRelease(sem_usb_rx);
|
osSemaphoreRelease(sem_usb_rx);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
const osThreadAttr_t usbServerTask_attributes = {
|
const osThreadAttr_t usbServerTask_attributes = {
|
||||||
.name = "UsbServerTask",
|
.name = "UsbServerTask",
|
||||||
.stack_size = 512 * 4,
|
.stack_size = 2000,
|
||||||
.priority = (osPriority_t) osPriorityNormal,
|
.priority = (osPriority_t) osPriorityNormal,
|
||||||
};
|
};
|
||||||
|
|
||||||
void StartUsbServer()
|
void StartUsbServer()
|
||||||
{
|
{
|
||||||
// Start USB communication thread
|
// Start USB communication thread
|
||||||
usbServerTaskHandle = osThreadNew(UsbServerTask, nullptr, &usbServerTask_attributes);
|
usbServerTaskHandle = osThreadNew(UsbServerTask, nullptr, &usbServerTask_attributes);
|
||||||
}
|
}
|
||||||
|
@ -1,114 +1,113 @@
|
|||||||
#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE!
|
#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE!
|
||||||
set(CMAKE_SYSTEM_NAME Generic)
|
set(CMAKE_SYSTEM_NAME Generic)
|
||||||
set(CMAKE_SYSTEM_VERSION 1)
|
set(CMAKE_SYSTEM_VERSION 1)
|
||||||
cmake_minimum_required(VERSION 3.19)
|
cmake_minimum_required(VERSION 3.19)
|
||||||
|
|
||||||
# specify cross compilers and tools
|
# specify cross compilers and tools
|
||||||
set(CMAKE_C_COMPILER arm-none-eabi-gcc)
|
set(CMAKE_C_COMPILER arm-none-eabi-gcc)
|
||||||
set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
|
set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
|
||||||
set(CMAKE_ASM_COMPILER arm-none-eabi-gcc)
|
set(CMAKE_ASM_COMPILER arm-none-eabi-gcc)
|
||||||
set(CMAKE_AR arm-none-eabi-ar)
|
set(CMAKE_AR arm-none-eabi-ar)
|
||||||
set(CMAKE_OBJCOPY arm-none-eabi-objcopy)
|
set(CMAKE_OBJCOPY arm-none-eabi-objcopy)
|
||||||
set(CMAKE_OBJDUMP arm-none-eabi-objdump)
|
set(CMAKE_OBJDUMP arm-none-eabi-objdump)
|
||||||
set(SIZE arm-none-eabi-size)
|
set(SIZE arm-none-eabi-size)
|
||||||
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
|
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
|
||||||
|
|
||||||
# project settings
|
# project settings
|
||||||
project(Core-STM32F4-fw C CXX ASM)
|
project(Core-STM32F4-fw C CXX ASM)
|
||||||
set(CMAKE_CXX_STANDARD 17)
|
set(CMAKE_CXX_STANDARD 17)
|
||||||
set(CMAKE_C_STANDARD 11)
|
set(CMAKE_C_STANDARD 11)
|
||||||
|
|
||||||
# for use printf & scanf with float
|
# for use printf & scanf with float
|
||||||
set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float")
|
set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float")
|
||||||
|
|
||||||
#Uncomment for hardware floating point
|
#Uncomment for hardware floating point
|
||||||
add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING)
|
add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING)
|
||||||
add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
|
add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
|
||||||
add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
|
add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
|
||||||
|
|
||||||
#Uncomment for software floating point
|
#Uncomment for software floating point
|
||||||
#add_compile_options(-mfloat-abi=soft)
|
#add_compile_options(-mfloat-abi=soft)
|
||||||
|
|
||||||
add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
|
add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
|
||||||
add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0)
|
add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0)
|
||||||
|
|
||||||
# uncomment to mitigate c++17 absolute addresses warnings
|
# uncomment to mitigate c++17 absolute addresses warnings
|
||||||
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register")
|
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register")
|
||||||
|
|
||||||
if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
|
if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
|
||||||
message(STATUS "Maximum optimization for speed")
|
message(STATUS "Maximum optimization for speed")
|
||||||
add_compile_options(-Ofast)
|
add_compile_options(-Ofast)
|
||||||
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
|
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
|
||||||
message(STATUS "Maximum optimization for speed, debug info included")
|
message(STATUS "Maximum optimization for speed, debug info included")
|
||||||
add_compile_options(-Ofast -g)
|
add_compile_options(-Ofast -g)
|
||||||
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
|
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
|
||||||
message(STATUS "Maximum optimization for size")
|
message(STATUS "Maximum optimization for size")
|
||||||
add_compile_options(-Os)
|
add_compile_options(-Os)
|
||||||
else ()
|
else ()
|
||||||
message(STATUS "Minimal optimization, debug info included")
|
message(STATUS "Minimal optimization, debug info included")
|
||||||
add_compile_options(-Og -g)
|
add_compile_options(-Og -g)
|
||||||
endif ()
|
endif ()
|
||||||
|
|
||||||
include_directories(
|
include_directories(
|
||||||
Core/Inc
|
Core/Inc
|
||||||
Drivers/STM32F4xx_HAL_Driver/Inc
|
Drivers/STM32F4xx_HAL_Driver/Inc
|
||||||
Drivers/STM32F4xx_HAL_Driver/Inc/Legacy
|
Drivers/STM32F4xx_HAL_Driver/Inc/Legacy
|
||||||
Drivers/CMSIS/Device/ST/STM32F4xx/Include
|
Drivers/CMSIS/Device/ST/STM32F4xx/Include
|
||||||
Drivers/CMSIS/Include
|
Drivers/CMSIS/Include
|
||||||
Middlewares/Third_Party/FreeRTOS/Source/include
|
Middlewares/Third_Party/FreeRTOS/Source/include
|
||||||
Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
|
Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
|
||||||
Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
|
Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
|
||||||
Middlewares/ST/STM32_USB_Device_Library/Core/Inc
|
Middlewares/ST/STM32_USB_Device_Library/Core/Inc
|
||||||
Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc
|
Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc
|
||||||
USB_DEVICE/App
|
USB_DEVICE/App
|
||||||
USB_DEVICE/Target
|
USB_DEVICE/Target
|
||||||
3rdParty/fibre/cpp/include
|
3rdParty/fibre/cpp/include
|
||||||
3rdParty/u8g2
|
3rdParty/u8g2
|
||||||
3rdParty/u8g2/cpp
|
3rdParty/u8g2/cpp
|
||||||
Bsp
|
Bsp
|
||||||
Bsp/imu
|
Bsp/imu
|
||||||
Bsp/imu/filters
|
Bsp/imu/filters
|
||||||
Bsp/communication
|
Bsp/communication
|
||||||
Bsp/memory
|
Bsp/memory
|
||||||
Bsp/utils
|
Bsp/utils
|
||||||
Bsp/gpio
|
Bsp/gpio
|
||||||
Bsp/utils/software_i2c
|
Bsp/utils/software_i2c
|
||||||
Bsp/utils/arm_math
|
Bsp/utils/arm_math
|
||||||
Robot
|
Robot
|
||||||
UserApp
|
UserApp
|
||||||
)
|
)
|
||||||
|
|
||||||
add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP)
|
add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP)
|
||||||
add_definitions(-DDEBUG_VIA_USB_SERIAL)
|
|
||||||
|
file(GLOB_RECURSE SOURCES
|
||||||
file(GLOB_RECURSE SOURCES
|
"startup/*.*"
|
||||||
"startup/*.*"
|
"Drivers/*.*"
|
||||||
"Drivers/*.*"
|
"Core/*.*"
|
||||||
"Core/*.*"
|
"UserApp/*.*"
|
||||||
"UserApp/*.*"
|
"3rdParty/*.*"
|
||||||
"3rdParty/*.*"
|
"Middlewares/*.*"
|
||||||
"Middlewares/*.*"
|
"USB_DEVICE/*.*"
|
||||||
"USB_DEVICE/*.*"
|
"Robot/*.*"
|
||||||
"Robot/*.*"
|
"Bsp/*.*"
|
||||||
"Bsp/*.*"
|
)
|
||||||
)
|
|
||||||
|
set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32F405RGTx_FLASH.ld)
|
||||||
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(-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(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
|
add_link_options(-T ${LINKER_SCRIPT})
|
||||||
add_link_options(-T ${LINKER_SCRIPT})
|
|
||||||
|
link_directories("Drivers/CMSIS/Lib")
|
||||||
link_directories("Drivers/CMSIS/Lib")
|
link_libraries("arm_cortexM4lf_math.a")
|
||||||
link_libraries("arm_cortexM4lf_math.a")
|
|
||||||
|
add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
|
||||||
add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
|
|
||||||
|
set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex)
|
||||||
set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex)
|
set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin)
|
||||||
set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin)
|
|
||||||
|
add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD
|
||||||
add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD
|
COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:${PROJECT_NAME}.elf> ${HEX_FILE}
|
||||||
COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:${PROJECT_NAME}.elf> ${HEX_FILE}
|
COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${PROJECT_NAME}.elf> ${BIN_FILE}
|
||||||
COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${PROJECT_NAME}.elf> ${BIN_FILE}
|
COMMENT "Building ${HEX_FILE}
|
||||||
COMMENT "Building ${HEX_FILE}
|
Building ${BIN_FILE}")
|
||||||
Building ${BIN_FILE}")
|
|
||||||
|
@ -1,114 +1,113 @@
|
|||||||
#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE!
|
#THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE!
|
||||||
set(CMAKE_SYSTEM_NAME Generic)
|
set(CMAKE_SYSTEM_NAME Generic)
|
||||||
set(CMAKE_SYSTEM_VERSION 1)
|
set(CMAKE_SYSTEM_VERSION 1)
|
||||||
cmake_minimum_required(VERSION 3.19)
|
cmake_minimum_required(VERSION 3.19)
|
||||||
|
|
||||||
# specify cross compilers and tools
|
# specify cross compilers and tools
|
||||||
set(CMAKE_C_COMPILER arm-none-eabi-gcc)
|
set(CMAKE_C_COMPILER arm-none-eabi-gcc)
|
||||||
set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
|
set(CMAKE_CXX_COMPILER arm-none-eabi-g++)
|
||||||
set(CMAKE_ASM_COMPILER arm-none-eabi-gcc)
|
set(CMAKE_ASM_COMPILER arm-none-eabi-gcc)
|
||||||
set(CMAKE_AR arm-none-eabi-ar)
|
set(CMAKE_AR arm-none-eabi-ar)
|
||||||
set(CMAKE_OBJCOPY arm-none-eabi-objcopy)
|
set(CMAKE_OBJCOPY arm-none-eabi-objcopy)
|
||||||
set(CMAKE_OBJDUMP arm-none-eabi-objdump)
|
set(CMAKE_OBJDUMP arm-none-eabi-objdump)
|
||||||
set(SIZE arm-none-eabi-size)
|
set(SIZE arm-none-eabi-size)
|
||||||
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
|
set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
|
||||||
|
|
||||||
# project settings
|
# project settings
|
||||||
project(Core-STM32F4-fw C CXX ASM)
|
project(Core-STM32F4-fw C CXX ASM)
|
||||||
set(CMAKE_CXX_STANDARD 17)
|
set(CMAKE_CXX_STANDARD 17)
|
||||||
set(CMAKE_C_STANDARD 11)
|
set(CMAKE_C_STANDARD 11)
|
||||||
|
|
||||||
# for use printf & scanf with float
|
# for use printf & scanf with float
|
||||||
set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float")
|
set(COMMON_FLAGS "-specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float")
|
||||||
|
|
||||||
#Uncomment for hardware floating point
|
#Uncomment for hardware floating point
|
||||||
add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING)
|
add_compile_definitions(ARM_MATH_CM4;ARM_MATH_MATRIX_CHECK;ARM_MATH_ROUNDING)
|
||||||
add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
|
add_compile_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
|
||||||
add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
|
add_link_options(-mfloat-abi=hard -mfpu=fpv4-sp-d16)
|
||||||
|
|
||||||
#Uncomment for software floating point
|
#Uncomment for software floating point
|
||||||
#add_compile_options(-mfloat-abi=soft)
|
#add_compile_options(-mfloat-abi=soft)
|
||||||
|
|
||||||
add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
|
add_compile_options(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
|
||||||
add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0)
|
add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-length=0)
|
||||||
|
|
||||||
# uncomment to mitigate c++17 absolute addresses warnings
|
# uncomment to mitigate c++17 absolute addresses warnings
|
||||||
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register")
|
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-register")
|
||||||
|
|
||||||
if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
|
if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
|
||||||
message(STATUS "Maximum optimization for speed")
|
message(STATUS "Maximum optimization for speed")
|
||||||
add_compile_options(-Ofast)
|
add_compile_options(-Ofast)
|
||||||
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
|
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo")
|
||||||
message(STATUS "Maximum optimization for speed, debug info included")
|
message(STATUS "Maximum optimization for speed, debug info included")
|
||||||
add_compile_options(-Ofast -g)
|
add_compile_options(-Ofast -g)
|
||||||
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
|
elseif ("${CMAKE_BUILD_TYPE}" STREQUAL "MinSizeRel")
|
||||||
message(STATUS "Maximum optimization for size")
|
message(STATUS "Maximum optimization for size")
|
||||||
add_compile_options(-Os)
|
add_compile_options(-Os)
|
||||||
else ()
|
else ()
|
||||||
message(STATUS "Minimal optimization, debug info included")
|
message(STATUS "Minimal optimization, debug info included")
|
||||||
add_compile_options(-Og -g)
|
add_compile_options(-Og -g)
|
||||||
endif ()
|
endif ()
|
||||||
|
|
||||||
include_directories(
|
include_directories(
|
||||||
Core/Inc
|
Core/Inc
|
||||||
Drivers/STM32F4xx_HAL_Driver/Inc
|
Drivers/STM32F4xx_HAL_Driver/Inc
|
||||||
Drivers/STM32F4xx_HAL_Driver/Inc/Legacy
|
Drivers/STM32F4xx_HAL_Driver/Inc/Legacy
|
||||||
Drivers/CMSIS/Device/ST/STM32F4xx/Include
|
Drivers/CMSIS/Device/ST/STM32F4xx/Include
|
||||||
Drivers/CMSIS/Include
|
Drivers/CMSIS/Include
|
||||||
Middlewares/Third_Party/FreeRTOS/Source/include
|
Middlewares/Third_Party/FreeRTOS/Source/include
|
||||||
Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
|
Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
|
||||||
Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
|
Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
|
||||||
Middlewares/ST/STM32_USB_Device_Library/Core/Inc
|
Middlewares/ST/STM32_USB_Device_Library/Core/Inc
|
||||||
Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc
|
Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc
|
||||||
USB_DEVICE/App
|
USB_DEVICE/App
|
||||||
USB_DEVICE/Target
|
USB_DEVICE/Target
|
||||||
3rdParty/fibre/cpp/include
|
3rdParty/fibre/cpp/include
|
||||||
3rdParty/u8g2
|
3rdParty/u8g2
|
||||||
3rdParty/u8g2/cpp
|
3rdParty/u8g2/cpp
|
||||||
Bsp
|
Bsp
|
||||||
Bsp/imu
|
Bsp/imu
|
||||||
Bsp/imu/filters
|
Bsp/imu/filters
|
||||||
Bsp/communication
|
Bsp/communication
|
||||||
Bsp/memory
|
Bsp/memory
|
||||||
Bsp/utils
|
Bsp/utils
|
||||||
Bsp/gpio
|
Bsp/gpio
|
||||||
Bsp/utils/software_i2c
|
Bsp/utils/software_i2c
|
||||||
Bsp/utils/arm_math
|
Bsp/utils/arm_math
|
||||||
Robot
|
Robot
|
||||||
UserApp
|
UserApp
|
||||||
)
|
)
|
||||||
|
|
||||||
add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP)
|
add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP)
|
||||||
# add_definitions(-DDEBUG_VIA_USB_SERIAL)
|
|
||||||
|
file(GLOB_RECURSE SOURCES
|
||||||
file(GLOB_RECURSE SOURCES
|
"startup/*.*"
|
||||||
"startup/*.*"
|
"Drivers/*.*"
|
||||||
"Drivers/*.*"
|
"Core/*.*"
|
||||||
"Core/*.*"
|
"UserApp/*.*"
|
||||||
"UserApp/*.*"
|
"3rdParty/*.*"
|
||||||
"3rdParty/*.*"
|
"Middlewares/*.*"
|
||||||
"Middlewares/*.*"
|
"USB_DEVICE/*.*"
|
||||||
"USB_DEVICE/*.*"
|
"Robot/*.*"
|
||||||
"Robot/*.*"
|
"Bsp/*.*"
|
||||||
"Bsp/*.*"
|
)
|
||||||
)
|
|
||||||
|
set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32F405RGTx_FLASH.ld)
|
||||||
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(-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(-mcpu=cortex-m4 -mthumb -mthumb-interwork)
|
add_link_options(-T ${LINKER_SCRIPT})
|
||||||
add_link_options(-T ${LINKER_SCRIPT})
|
|
||||||
|
link_directories("Drivers/CMSIS/Lib")
|
||||||
link_directories("Drivers/CMSIS/Lib")
|
link_libraries("arm_cortexM4lf_math.a")
|
||||||
link_libraries("arm_cortexM4lf_math.a")
|
|
||||||
|
add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
|
||||||
add_executable(${PROJECT_NAME}.elf ${SOURCES} ${LINKER_SCRIPT})
|
|
||||||
|
set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex)
|
||||||
set(HEX_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.hex)
|
set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin)
|
||||||
set(BIN_FILE ${PROJECT_BINARY_DIR}/${PROJECT_NAME}.bin)
|
|
||||||
|
add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD
|
||||||
add_custom_command(TARGET ${PROJECT_NAME}.elf POST_BUILD
|
COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:${PROJECT_NAME}.elf> ${HEX_FILE}
|
||||||
COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:${PROJECT_NAME}.elf> ${HEX_FILE}
|
COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${PROJECT_NAME}.elf> ${BIN_FILE}
|
||||||
COMMAND ${CMAKE_OBJCOPY} -Obinary $<TARGET_FILE:${PROJECT_NAME}.elf> ${BIN_FILE}
|
COMMENT "Building ${HEX_FILE}
|
||||||
COMMENT "Building ${HEX_FILE}
|
Building ${BIN_FILE}")
|
||||||
Building ${BIN_FILE}")
|
|
||||||
|
@ -1,175 +1,175 @@
|
|||||||
/* USER CODE BEGIN Header */
|
/* USER CODE BEGIN Header */
|
||||||
/**
|
/**
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
* File Name : freertos.c
|
* File Name : freertos.c
|
||||||
* Description : Code for freertos applications
|
* Description : Code for freertos applications
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
* @attention
|
* @attention
|
||||||
*
|
*
|
||||||
* <h2><center>© Copyright (c) 2021 STMicroelectronics.
|
* <h2><center>© Copyright (c) 2021 STMicroelectronics.
|
||||||
* All rights reserved.</center></h2>
|
* All rights reserved.</center></h2>
|
||||||
*
|
*
|
||||||
* This software component is licensed by ST under Ultimate Liberty license
|
* This software component is licensed by ST under Ultimate Liberty license
|
||||||
* SLA0044, the "License"; You may not use this file except in compliance with
|
* SLA0044, the "License"; You may not use this file except in compliance with
|
||||||
* the License. You may obtain a copy of the License at:
|
* the License. You may obtain a copy of the License at:
|
||||||
* www.st.com/SLA0044
|
* www.st.com/SLA0044
|
||||||
*
|
*
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
*/
|
*/
|
||||||
/* USER CODE END Header */
|
/* USER CODE END Header */
|
||||||
|
|
||||||
/* Includes ------------------------------------------------------------------*/
|
/* Includes ------------------------------------------------------------------*/
|
||||||
#include "FreeRTOS.h"
|
#include "FreeRTOS.h"
|
||||||
#include "task.h"
|
#include "task.h"
|
||||||
#include "main.h"
|
#include "main.h"
|
||||||
#include "cmsis_os.h"
|
#include "cmsis_os.h"
|
||||||
|
|
||||||
/* Private includes ----------------------------------------------------------*/
|
/* Private includes ----------------------------------------------------------*/
|
||||||
/* USER CODE BEGIN Includes */
|
/* USER CODE BEGIN Includes */
|
||||||
#include "common_inc.h"
|
#include "common_inc.h"
|
||||||
#include "communication.hpp"
|
#include "communication.hpp"
|
||||||
/* USER CODE END Includes */
|
/* USER CODE END Includes */
|
||||||
|
|
||||||
/* Private typedef -----------------------------------------------------------*/
|
/* Private typedef -----------------------------------------------------------*/
|
||||||
/* USER CODE BEGIN PTD */
|
/* USER CODE BEGIN PTD */
|
||||||
|
|
||||||
/* USER CODE END PTD */
|
/* USER CODE END PTD */
|
||||||
|
|
||||||
/* Private define ------------------------------------------------------------*/
|
/* Private define ------------------------------------------------------------*/
|
||||||
/* USER CODE BEGIN PD */
|
/* USER CODE BEGIN PD */
|
||||||
|
|
||||||
/* USER CODE END PD */
|
/* USER CODE END PD */
|
||||||
|
|
||||||
/* Private macro -------------------------------------------------------------*/
|
/* Private macro -------------------------------------------------------------*/
|
||||||
/* USER CODE BEGIN PM */
|
/* USER CODE BEGIN PM */
|
||||||
|
|
||||||
/* USER CODE END PM */
|
/* USER CODE END PM */
|
||||||
|
|
||||||
/* Private variables ---------------------------------------------------------*/
|
/* Private variables ---------------------------------------------------------*/
|
||||||
/* USER CODE BEGIN Variables */
|
/* USER CODE BEGIN Variables */
|
||||||
|
|
||||||
// List of semaphores
|
// List of semaphores
|
||||||
osSemaphoreId sem_usb_irq;
|
osSemaphoreId sem_usb_irq;
|
||||||
osSemaphoreId sem_uart4_dma;
|
osSemaphoreId sem_uart4_dma;
|
||||||
osSemaphoreId sem_uart5_dma;
|
osSemaphoreId sem_uart5_dma;
|
||||||
osSemaphoreId sem_usb_rx;
|
osSemaphoreId sem_usb_rx;
|
||||||
osSemaphoreId sem_usb_tx;
|
osSemaphoreId sem_usb_tx;
|
||||||
osSemaphoreId sem_can1_tx;
|
osSemaphoreId sem_can1_tx;
|
||||||
osSemaphoreId sem_can2_tx;
|
osSemaphoreId sem_can2_tx;
|
||||||
|
|
||||||
/* USER CODE END Variables */
|
/* USER CODE END Variables */
|
||||||
/* Definitions for defaultTask */
|
/* Definitions for defaultTask */
|
||||||
osThreadId_t defaultTaskHandle;
|
osThreadId_t defaultTaskHandle;
|
||||||
const osThreadAttr_t defaultTask_attributes = {
|
const osThreadAttr_t defaultTask_attributes = {
|
||||||
.name = "defaultTask",
|
.name = "defaultTask",
|
||||||
.stack_size = 500 * 4,
|
.stack_size = 2000,
|
||||||
.priority = (osPriority_t) osPriorityNormal,
|
.priority = (osPriority_t) osPriorityNormal,
|
||||||
};
|
};
|
||||||
|
|
||||||
/* Private function prototypes -----------------------------------------------*/
|
/* Private function prototypes -----------------------------------------------*/
|
||||||
/* USER CODE BEGIN FunctionPrototypes */
|
/* USER CODE BEGIN FunctionPrototypes */
|
||||||
|
|
||||||
|
|
||||||
/* USER CODE END FunctionPrototypes */
|
/* USER CODE END FunctionPrototypes */
|
||||||
|
|
||||||
void StartDefaultTask(void *argument);
|
void StartDefaultTask(void *argument);
|
||||||
|
|
||||||
extern void MX_USB_DEVICE_Init(void);
|
extern void MX_USB_DEVICE_Init(void);
|
||||||
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
|
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief FreeRTOS initialization
|
* @brief FreeRTOS initialization
|
||||||
* @param None
|
* @param None
|
||||||
* @retval None
|
* @retval None
|
||||||
*/
|
*/
|
||||||
void MX_FREERTOS_Init(void) {
|
void MX_FREERTOS_Init(void) {
|
||||||
/* USER CODE BEGIN Init */
|
/* USER CODE BEGIN Init */
|
||||||
|
|
||||||
/* USER CODE END Init */
|
/* USER CODE END Init */
|
||||||
|
|
||||||
/* USER CODE BEGIN RTOS_MUTEX */
|
/* USER CODE BEGIN RTOS_MUTEX */
|
||||||
/* add mutexes, ... */
|
/* add mutexes, ... */
|
||||||
/* USER CODE END RTOS_MUTEX */
|
/* USER CODE END RTOS_MUTEX */
|
||||||
|
|
||||||
/* USER CODE BEGIN RTOS_SEMAPHORES */
|
/* USER CODE BEGIN RTOS_SEMAPHORES */
|
||||||
// Init usb irq binary semaphore, and start with no tokens by removing the starting one.
|
// Init usb irq binary semaphore, and start with no tokens by removing the starting one.
|
||||||
osSemaphoreDef(sem_usb_irq);
|
osSemaphoreDef(sem_usb_irq);
|
||||||
sem_usb_irq = osSemaphoreNew(1, 0, osSemaphore(sem_usb_irq));
|
sem_usb_irq = osSemaphoreNew(1, 0, osSemaphore(sem_usb_irq));
|
||||||
|
|
||||||
// Create a semaphore for UART DMA and remove a token
|
// Create a semaphore for UART DMA and remove a token
|
||||||
osSemaphoreDef(sem_uart4_dma);
|
osSemaphoreDef(sem_uart4_dma);
|
||||||
sem_uart4_dma = osSemaphoreNew(1, 1, osSemaphore(sem_uart4_dma));
|
sem_uart4_dma = osSemaphoreNew(1, 1, osSemaphore(sem_uart4_dma));
|
||||||
osSemaphoreDef(sem_uart5_dma);
|
osSemaphoreDef(sem_uart5_dma);
|
||||||
sem_uart5_dma = osSemaphoreNew(1, 1, osSemaphore(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.
|
// Create a semaphore for USB RX, and start with no tokens by removing the starting one.
|
||||||
osSemaphoreDef(sem_usb_rx);
|
osSemaphoreDef(sem_usb_rx);
|
||||||
sem_usb_rx = osSemaphoreNew(1, 0, osSemaphore(sem_usb_rx));
|
sem_usb_rx = osSemaphoreNew(1, 0, osSemaphore(sem_usb_rx));
|
||||||
|
|
||||||
// Create a semaphore for USB TX
|
// Create a semaphore for USB TX
|
||||||
osSemaphoreDef(sem_usb_tx);
|
osSemaphoreDef(sem_usb_tx);
|
||||||
sem_usb_tx = osSemaphoreNew(1, 1, osSemaphore(sem_usb_tx));
|
sem_usb_tx = osSemaphoreNew(1, 1, osSemaphore(sem_usb_tx));
|
||||||
|
|
||||||
// Create a semaphore for CAN TX
|
// Create a semaphore for CAN TX
|
||||||
osSemaphoreDef(sem_can1_tx);
|
osSemaphoreDef(sem_can1_tx);
|
||||||
sem_can1_tx = osSemaphoreNew(1, 1, osSemaphore(sem_can1_tx));
|
sem_can1_tx = osSemaphoreNew(1, 1, osSemaphore(sem_can1_tx));
|
||||||
osSemaphoreDef(sem_can2_tx);
|
osSemaphoreDef(sem_can2_tx);
|
||||||
sem_can2_tx = osSemaphoreNew(1, 1, osSemaphore(sem_can2_tx));
|
sem_can2_tx = osSemaphoreNew(1, 1, osSemaphore(sem_can2_tx));
|
||||||
|
|
||||||
/* USER CODE END RTOS_SEMAPHORES */
|
/* USER CODE END RTOS_SEMAPHORES */
|
||||||
|
|
||||||
/* USER CODE BEGIN RTOS_TIMERS */
|
/* USER CODE BEGIN RTOS_TIMERS */
|
||||||
|
|
||||||
/* USER CODE END RTOS_TIMERS */
|
/* USER CODE END RTOS_TIMERS */
|
||||||
|
|
||||||
/* USER CODE BEGIN RTOS_QUEUES */
|
/* USER CODE BEGIN RTOS_QUEUES */
|
||||||
// This Task must run before MX_USB_DEVICE_Init(), so have to put it here.
|
// This Task must run before MX_USB_DEVICE_Init(), so have to put it here.
|
||||||
const osThreadAttr_t usbIrqTask_attributes = {
|
const osThreadAttr_t usbIrqTask_attributes = {
|
||||||
.name = "usbIrqTask",
|
.name = "usbIrqTask",
|
||||||
.stack_size = 128 * 4,
|
.stack_size = 500,
|
||||||
.priority = (osPriority_t) osPriorityAboveNormal,
|
.priority = (osPriority_t) osPriorityAboveNormal,
|
||||||
};
|
};
|
||||||
usbIrqTaskHandle = osThreadNew(UsbDeferredInterruptTask, NULL, &usbIrqTask_attributes);
|
usbIrqTaskHandle = osThreadNew(UsbDeferredInterruptTask, NULL, &usbIrqTask_attributes);
|
||||||
|
|
||||||
/* USER CODE END RTOS_QUEUES */
|
/* USER CODE END RTOS_QUEUES */
|
||||||
|
|
||||||
/* Create the thread(s) */
|
/* Create the thread(s) */
|
||||||
/* creation of defaultTask */
|
/* creation of defaultTask */
|
||||||
defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);
|
defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);
|
||||||
|
|
||||||
/* USER CODE BEGIN RTOS_THREADS */
|
/* USER CODE BEGIN RTOS_THREADS */
|
||||||
/* add threads, ... */
|
/* add threads, ... */
|
||||||
/* USER CODE END RTOS_THREADS */
|
/* USER CODE END RTOS_THREADS */
|
||||||
|
|
||||||
/* USER CODE BEGIN RTOS_EVENTS */
|
/* USER CODE BEGIN RTOS_EVENTS */
|
||||||
/* add events, ... */
|
/* add events, ... */
|
||||||
/* USER CODE END RTOS_EVENTS */
|
/* USER CODE END RTOS_EVENTS */
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* USER CODE BEGIN Header_StartDefaultTask */
|
/* USER CODE BEGIN Header_StartDefaultTask */
|
||||||
/**
|
/**
|
||||||
* @brief Function implementing the defaultTask thread.
|
* @brief Function implementing the defaultTask thread.
|
||||||
* @param argument: Not used
|
* @param argument: Not used
|
||||||
* @retval None
|
* @retval None
|
||||||
*/
|
*/
|
||||||
/* USER CODE END Header_StartDefaultTask */
|
/* USER CODE END Header_StartDefaultTask */
|
||||||
void StartDefaultTask(void *argument)
|
void StartDefaultTask(void *argument)
|
||||||
{
|
{
|
||||||
/* init code for USB_DEVICE */
|
/* init code for USB_DEVICE */
|
||||||
MX_USB_DEVICE_Init();
|
MX_USB_DEVICE_Init();
|
||||||
/* USER CODE BEGIN StartDefaultTask */
|
/* USER CODE BEGIN StartDefaultTask */
|
||||||
|
|
||||||
// Invoke cpp-version main().
|
// Invoke cpp-version main().
|
||||||
Main();
|
Main();
|
||||||
|
|
||||||
vTaskDelete(defaultTaskHandle);
|
vTaskDelete(defaultTaskHandle);
|
||||||
/* USER CODE END StartDefaultTask */
|
/* USER CODE END StartDefaultTask */
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Private application code --------------------------------------------------*/
|
/* Private application code --------------------------------------------------*/
|
||||||
/* USER CODE BEGIN Application */
|
/* USER CODE BEGIN Application */
|
||||||
|
|
||||||
/* USER CODE END Application */
|
/* USER CODE END Application */
|
||||||
|
|
||||||
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|
/************************ (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 "ctrl_step.hpp"
|
||||||
#include "communication.hpp"
|
#include "communication.hpp"
|
||||||
|
|
||||||
|
|
||||||
CtrlStepMotor::CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse,
|
CtrlStepMotor::CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse,
|
||||||
uint8_t _reduction, float _minAngle, float _maxAngle) :
|
uint8_t _reduction, float _angleLimitMin, float _angleLimitMax) :
|
||||||
nodeID(_id), hcan(_hcan), inverseDirection(_inverse), reduction(_reduction),
|
nodeID(_id), hcan(_hcan), inverseDirection(_inverse), reduction(_reduction),
|
||||||
angleLimitMin(_minAngle), angleLimitMax(_maxAngle)
|
angleLimitMin(_angleLimitMin), angleLimitMax(_angleLimitMax)
|
||||||
{
|
{
|
||||||
txHeader =
|
txHeader =
|
||||||
{
|
{
|
||||||
.StdId = 0,
|
.StdId = 0,
|
||||||
.ExtId = 0,
|
.ExtId = 0,
|
||||||
.IDE = CAN_ID_STD,
|
.IDE = CAN_ID_STD,
|
||||||
.RTR = CAN_RTR_DATA,
|
.RTR = CAN_RTR_DATA,
|
||||||
.DLC = 8,
|
.DLC = 8,
|
||||||
.TransmitGlobalTime = DISABLE
|
.TransmitGlobalTime = DISABLE
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void CtrlStepMotor::SetEnable(bool _enable)
|
void CtrlStepMotor::SetEnable(bool _enable)
|
||||||
{
|
{
|
||||||
state = _enable ? FINISH : STOP;
|
state = _enable ? FINISH : STOP;
|
||||||
|
|
||||||
uint8_t mode = 0x01;
|
uint8_t mode = 0x01;
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
|
|
||||||
// Int to Bytes
|
// Int to Bytes
|
||||||
uint32_t val = _enable ? 1 : 0;
|
uint32_t val = _enable ? 1 : 0;
|
||||||
auto* b = (unsigned char*) &val;
|
auto* b = (unsigned char*) &val;
|
||||||
for (int i = 0; i < 4; i++)
|
for (int i = 0; i < 4; i++)
|
||||||
canBuf[i] = *(b + i);
|
canBuf[i] = *(b + i);
|
||||||
|
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void CtrlStepMotor::DoCalibration()
|
void CtrlStepMotor::DoCalibration()
|
||||||
{
|
{
|
||||||
uint8_t mode = 0x02;
|
uint8_t mode = 0x02;
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
|
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void CtrlStepMotor::SetCurrentSetPoint(float _val)
|
void CtrlStepMotor::SetCurrentSetPoint(float _val)
|
||||||
{
|
{
|
||||||
state = RUNNING;
|
state = RUNNING;
|
||||||
|
|
||||||
uint8_t mode = 0x03;
|
uint8_t mode = 0x03;
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
|
|
||||||
// Float to Bytes
|
// Float to Bytes
|
||||||
auto* b = (unsigned char*) &_val;
|
auto* b = (unsigned char*) &_val;
|
||||||
for (int i = 0; i < 4; i++)
|
for (int i = 0; i < 4; i++)
|
||||||
canBuf[i] = *(b + i);
|
canBuf[i] = *(b + i);
|
||||||
|
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void CtrlStepMotor::SetVelocitySetPoint(float _val)
|
void CtrlStepMotor::SetVelocitySetPoint(float _val)
|
||||||
{
|
{
|
||||||
state = RUNNING;
|
state = RUNNING;
|
||||||
|
|
||||||
uint8_t mode = 0x04;
|
uint8_t mode = 0x04;
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
|
|
||||||
// Float to Bytes
|
// Float to Bytes
|
||||||
auto* b = (unsigned char*) &_val;
|
auto* b = (unsigned char*) &_val;
|
||||||
for (int i = 0; i < 4; i++)
|
for (int i = 0; i < 4; i++)
|
||||||
canBuf[i] = *(b + i);
|
canBuf[i] = *(b + i);
|
||||||
|
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void CtrlStepMotor::SetPositionSetPoint(float _val)
|
void CtrlStepMotor::SetPositionSetPoint(float _val)
|
||||||
{
|
{
|
||||||
state = RUNNING;
|
uint8_t mode = 0x05;
|
||||||
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
uint8_t mode = 0x05;
|
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
// Float to Bytes
|
||||||
|
auto* b = (unsigned char*) &_val;
|
||||||
// Float to Bytes
|
for (int i = 0; i < 4; i++)
|
||||||
auto* b = (unsigned char*) &_val;
|
canBuf[i] = *(b + i);
|
||||||
for (int i = 0; i < 4; i++)
|
canBuf[4] = 1; // Need ACK
|
||||||
canBuf[i] = *(b + i);
|
|
||||||
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
}
|
||||||
}
|
|
||||||
|
|
||||||
|
void CtrlStepMotor::SetPositionWithVelocityLimit(float _pos, float _vel)
|
||||||
void CtrlStepMotor::SetPositionWithTime(float _pos, float _time)
|
{
|
||||||
{
|
uint8_t mode = 0x07;
|
||||||
state = RUNNING;
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
|
|
||||||
uint8_t mode = 0x06;
|
// Float to Bytes
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
auto* b = (unsigned char*) &_pos;
|
||||||
|
for (int i = 0; i < 4; i++)
|
||||||
// Float to Bytes
|
canBuf[i] = *(b + i);
|
||||||
auto* b = (unsigned char*) &_pos;
|
b = (unsigned char*) &_vel;
|
||||||
for (int i = 0; i < 4; i++)
|
for (int i = 4; i < 8; i++)
|
||||||
canBuf[i] = *(b + i);
|
canBuf[i] = *(b + i - 4);
|
||||||
|
|
||||||
b = (unsigned char*) &_time;
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
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;
|
||||||
void CtrlStepMotor::AddTrajectoryPoint(float _pos, float _vel)
|
|
||||||
{
|
// Int to Bytes
|
||||||
uint8_t mode = 0x07;
|
auto* b = (unsigned char*) &_id;
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
for (int i = 0; i < 4; i++)
|
||||||
|
canBuf[i] = *(b + i);
|
||||||
// Float to Bytes
|
canBuf[4] = 1; // Need save to EEPROM or not
|
||||||
auto* b = (unsigned char*) &_pos;
|
|
||||||
for (int i = 0; i < 4; i++)
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
canBuf[i] = *(b + i);
|
}
|
||||||
|
|
||||||
b = (unsigned char*) &_vel;
|
|
||||||
for (int i = 4; i < 8; i++)
|
void CtrlStepMotor::SetCurrentLimit(float _val)
|
||||||
canBuf[i] = *(b + i - 4);
|
{
|
||||||
|
uint8_t mode = 0x12;
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
}
|
|
||||||
|
// Float to Bytes
|
||||||
|
auto* b = (unsigned char*) &_val;
|
||||||
void CtrlStepMotor::SetNodeID(uint32_t _id)
|
for (int i = 0; i < 4; i++)
|
||||||
{
|
canBuf[i] = *(b + i);
|
||||||
uint8_t mode = 0x11;
|
canBuf[4] = 1; // Need save to EEPROM or not
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
|
||||||
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
// Int to Bytes
|
}
|
||||||
auto* b = (unsigned char*) &_id;
|
|
||||||
for (int i = 0; i < 4; i++)
|
|
||||||
canBuf[i] = *(b + i);
|
void CtrlStepMotor::SetVelocityLimit(float _val)
|
||||||
|
{
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
uint8_t mode = 0x13;
|
||||||
}
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
|
|
||||||
|
// Float to Bytes
|
||||||
void CtrlStepMotor::SetCurrentLimit(float _val)
|
auto* b = (unsigned char*) &_val;
|
||||||
{
|
for (int i = 0; i < 4; i++)
|
||||||
uint8_t mode = 0x12;
|
canBuf[i] = *(b + i);
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
canBuf[4] = 1; // Need save to EEPROM or not
|
||||||
|
|
||||||
// Float to Bytes
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
auto* b = (unsigned char*) &_val;
|
}
|
||||||
for (int i = 0; i < 4; i++)
|
|
||||||
canBuf[i] = *(b + i);
|
|
||||||
|
void CtrlStepMotor::SetAcceleration(float _val)
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
{
|
||||||
}
|
uint8_t mode = 0x14;
|
||||||
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
|
|
||||||
void CtrlStepMotor::SetVelocityLimit(float _val)
|
// Float to Bytes
|
||||||
{
|
auto* b = (unsigned char*) &_val;
|
||||||
uint8_t mode = 0x13;
|
for (int i = 0; i < 4; i++)
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
canBuf[i] = *(b + i);
|
||||||
|
canBuf[4] = 0; // Need save to EEPROM or not
|
||||||
// Float to Bytes
|
|
||||||
auto* b = (unsigned char*) &_val;
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
for (int i = 0; i < 4; i++)
|
}
|
||||||
canBuf[i] = *(b + i);
|
|
||||||
|
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
void CtrlStepMotor::ApplyPositionAsHome()
|
||||||
}
|
{
|
||||||
|
uint8_t mode = 0x15;
|
||||||
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
void CtrlStepMotor::SetAcceleration(float _val, bool _storeToMem)
|
|
||||||
{
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
uint8_t mode = 0x14;
|
}
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
|
||||||
|
|
||||||
// Float to Bytes
|
void CtrlStepMotor::SetEnableOnBoot(bool _enable)
|
||||||
auto* b = (unsigned char*) &_val;
|
{
|
||||||
for (int i = 0; i < 4; i++)
|
uint8_t mode = 0x16;
|
||||||
canBuf[i] = *(b + i);
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
canBuf[4] = _storeToMem ? 1 : 0;
|
|
||||||
|
// Int to Bytes
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
uint32_t val = _enable ? 1 : 0;
|
||||||
}
|
auto* b = (unsigned char*) &val;
|
||||||
|
for (int i = 0; i < 4; i++)
|
||||||
|
canBuf[i] = *(b + i);
|
||||||
void CtrlStepMotor::ApplyPositionAsHome()
|
canBuf[4] = 1; // Need save to EEPROM or not
|
||||||
{
|
|
||||||
uint8_t mode = 0x15;
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
}
|
||||||
|
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
|
||||||
}
|
void CtrlStepMotor::SetEnableStallProtect(bool _enable)
|
||||||
|
{
|
||||||
|
uint8_t mode = 0x1B;
|
||||||
void CtrlStepMotor::SetEnableOnBoot(bool _enable)
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
{
|
|
||||||
uint8_t mode = 0x16;
|
uint32_t val = _enable ? 1 : 0;
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
auto* b = (unsigned char*) &val;
|
||||||
|
for (int i = 0; i < 4; i++)
|
||||||
// Int to Bytes
|
canBuf[i] = *(b + i);
|
||||||
uint32_t val = _enable ? 1 : 0;
|
canBuf[4] = 1; // Need save to EEPROM or not
|
||||||
auto* b = (unsigned char*) &val;
|
|
||||||
for (int i = 0; i < 4; i++)
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
canBuf[i] = *(b + i);
|
}
|
||||||
|
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
|
||||||
}
|
void CtrlStepMotor::Reboot()
|
||||||
|
{
|
||||||
|
uint8_t mode = 0x7f;
|
||||||
void CtrlStepMotor::SetEnableAck(bool _enable)
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
{
|
|
||||||
uint8_t mode = 0x1B;
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
}
|
||||||
|
|
||||||
uint32_t val = _enable ? 1 : 0;
|
|
||||||
auto* b = (unsigned char*) &val;
|
void CtrlStepMotor::EraseConfigs()
|
||||||
for (int i = 0; i < 4; i++)
|
{
|
||||||
canBuf[i] = *(b + i);
|
uint8_t mode = 0x7e;
|
||||||
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
|
||||||
}
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
|
}
|
||||||
|
|
||||||
void CtrlStepMotor::SetEnableStallProtect(bool _enable)
|
|
||||||
{
|
void CtrlStepMotor::SetAngle(float _angle)
|
||||||
uint8_t mode = 0x1C;
|
{
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
_angle = inverseDirection ? -_angle : _angle;
|
||||||
|
float stepMotorCnt = _angle / 360.0f * (float) reduction;
|
||||||
uint32_t val = _enable ? 1 : 0;
|
SetPositionSetPoint(stepMotorCnt);
|
||||||
auto* b = (unsigned char*) &val;
|
}
|
||||||
for (int i = 0; i < 4; i++)
|
|
||||||
canBuf[i] = *(b + i);
|
|
||||||
|
void CtrlStepMotor::SetAngleWithVelocityLimit(float _angle, float _vel)
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
{
|
||||||
}
|
_angle = inverseDirection ? -_angle : _angle;
|
||||||
|
float stepMotorCnt = _angle / 360.0f * (float) reduction;
|
||||||
|
SetPositionWithVelocityLimit(stepMotorCnt, _vel);
|
||||||
void CtrlStepMotor::Reboot()
|
}
|
||||||
{
|
|
||||||
uint8_t mode = 0x7f;
|
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
void CtrlStepMotor::UpdateAngle()
|
||||||
|
{
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
uint8_t mode = 0x23;
|
||||||
}
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
|
|
||||||
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
void CtrlStepMotor::EraseConfigs()
|
}
|
||||||
{
|
|
||||||
uint8_t mode = 0x7e;
|
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
void CtrlStepMotor::UpdateAngleCallback(float _pos, bool _isFinished)
|
||||||
|
{
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
state = _isFinished ? FINISH : RUNNING;
|
||||||
}
|
|
||||||
|
float tmp = _pos / (float) reduction * 360;
|
||||||
|
angle = inverseDirection ? -tmp : tmp;
|
||||||
void CtrlStepMotor::SetAngle(float _angle)
|
}
|
||||||
{
|
|
||||||
_angle = inverseDirection ? -_angle : _angle;
|
|
||||||
if (_angle <= angleLimitMax && _angle >= angleLimitMin)
|
void CtrlStepMotor::SetDceKp(int32_t _val)
|
||||||
{
|
{
|
||||||
float stepMotorCnt = _angle / 360.0f * reduction * CTRL_CIRCLE_COUNT;
|
uint8_t mode = 0x17;
|
||||||
SetPositionSetPoint(stepMotorCnt);
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
}
|
|
||||||
}
|
auto* b = (unsigned char*) &_val;
|
||||||
|
for (int i = 0; i < 4; i++)
|
||||||
|
canBuf[i] = *(b + i);
|
||||||
void CtrlStepMotor::SetAngleWithTime(float _angle, float _time)
|
canBuf[4] = 1; // Need save to EEPROM or not
|
||||||
{
|
|
||||||
_angle = inverseDirection ? -_angle : _angle;
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
float stepMotorCnt = _angle / 360.0f * reduction;
|
}
|
||||||
SetPositionWithTime(stepMotorCnt, _time);
|
|
||||||
}
|
|
||||||
|
void CtrlStepMotor::SetDceKv(int32_t _val)
|
||||||
|
{
|
||||||
void CtrlStepMotor::UpdateAngle()
|
uint8_t mode = 0x18;
|
||||||
{
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
uint8_t mode = 0x23;
|
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
auto* b = (unsigned char*) &_val;
|
||||||
|
for (int i = 0; i < 4; i++)
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
canBuf[i] = *(b + i);
|
||||||
}
|
canBuf[4] = 1; // Need save to EEPROM or not
|
||||||
|
|
||||||
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
void CtrlStepMotor::UpdateAngleCallback(float _pos, bool _isAck)
|
}
|
||||||
{
|
|
||||||
if (_isAck)
|
|
||||||
state = FINISH;
|
void CtrlStepMotor::SetDceKi(int32_t _val)
|
||||||
|
{
|
||||||
float tmp = _pos / (float) reduction * 360;
|
uint8_t mode = 0x19;
|
||||||
angle = inverseDirection ? -tmp : tmp;
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
}
|
|
||||||
|
auto* b = (unsigned char*) &_val;
|
||||||
|
for (int i = 0; i < 4; i++)
|
||||||
void CtrlStepMotor::SetDceKp(int32_t _val)
|
canBuf[i] = *(b + i);
|
||||||
{
|
canBuf[4] = 1; // Need save to EEPROM or not
|
||||||
uint8_t mode = 0x17;
|
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
|
}
|
||||||
auto* b = (unsigned char*) &_val;
|
|
||||||
for (int i = 0; i < 4; i++)
|
|
||||||
canBuf[i] = *(b + i);
|
void CtrlStepMotor::SetDceKd(int32_t _val)
|
||||||
|
{
|
||||||
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
uint8_t mode = 0x1A;
|
||||||
}
|
txHeader.StdId = nodeID << 7 | mode;
|
||||||
|
|
||||||
|
auto* b = (unsigned char*) &_val;
|
||||||
void CtrlStepMotor::SetDceKv(int32_t _val)
|
for (int i = 0; i < 4; i++)
|
||||||
{
|
canBuf[i] = *(b + i);
|
||||||
uint8_t mode = 0x18;
|
canBuf[4] = 1; // Need save to EEPROM or not
|
||||||
txHeader.StdId = nodeID << 7 | mode;
|
|
||||||
|
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
@ -1,99 +1,95 @@
|
|||||||
#ifndef DUMMY_CORE_FW_CTRL_STEP_HPP
|
#ifndef DUMMY_CORE_FW_CTRL_STEP_HPP
|
||||||
#define DUMMY_CORE_FW_CTRL_STEP_HPP
|
#define DUMMY_CORE_FW_CTRL_STEP_HPP
|
||||||
|
|
||||||
#include "fibre/protocol.hpp"
|
#include "fibre/protocol.hpp"
|
||||||
#include "can.h"
|
#include "can.h"
|
||||||
|
|
||||||
class CtrlStepMotor
|
class CtrlStepMotor
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
enum State
|
enum State
|
||||||
{
|
{
|
||||||
RUNNING,
|
RUNNING,
|
||||||
FINISH,
|
FINISH,
|
||||||
STOP
|
STOP
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
const uint32_t CTRL_CIRCLE_COUNT = 200 * 256;
|
const uint32_t CTRL_CIRCLE_COUNT = 200 * 256;
|
||||||
|
|
||||||
CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse = false, uint8_t _reduction = 1,
|
CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse = false, uint8_t _reduction = 1,
|
||||||
float _minAngle = -180, float _maxAngle = 180);
|
float _angleLimitMin = -180, float _angleLimitMax = 180);
|
||||||
|
|
||||||
uint8_t nodeID;
|
uint8_t nodeID;
|
||||||
float angle = 0;
|
float angle = 0;
|
||||||
float angleLimitMax;
|
float angleLimitMax;
|
||||||
float angleLimitMin;
|
float angleLimitMin;
|
||||||
bool inverseDirection;
|
bool inverseDirection;
|
||||||
uint8_t reduction;
|
uint8_t reduction;
|
||||||
State state = STOP;
|
State state = STOP;
|
||||||
|
|
||||||
void SetAngle(float _angle);
|
void SetAngle(float _angle);
|
||||||
void SetAngleWithTime(float _angle, float _time);
|
void SetAngleWithVelocityLimit(float _angle, float _vel);
|
||||||
// CAN Command
|
// CAN Command
|
||||||
void SetEnable(bool _enable);
|
void SetEnable(bool _enable);
|
||||||
void DoCalibration();
|
void DoCalibration();
|
||||||
void SetCurrentSetPoint(float _val);
|
void SetCurrentSetPoint(float _val);
|
||||||
void SetVelocitySetPoint(float _val);
|
void SetVelocitySetPoint(float _val);
|
||||||
void SetPositionSetPoint(float _val);
|
void SetPositionSetPoint(float _val);
|
||||||
void SetPositionWithTime(float _pos, float _time);
|
void SetPositionWithVelocityLimit(float _pos, float _vel);
|
||||||
void AddTrajectoryPoint(float _pos, float _vel);
|
void SetNodeID(uint32_t _id);
|
||||||
void SetNodeID(uint32_t _id);
|
void SetCurrentLimit(float _val);
|
||||||
void SetCurrentLimit(float _val);
|
void SetVelocityLimit(float _val);
|
||||||
void SetVelocityLimit(float _val);
|
void SetAcceleration(float _val);
|
||||||
void SetAcceleration(float _val, bool _storeToMem = false);
|
void SetDceKp(int32_t _val);
|
||||||
void SetDceKp(int32_t _val);
|
void SetDceKv(int32_t _val);
|
||||||
void SetDceKv(int32_t _val);
|
void SetDceKi(int32_t _val);
|
||||||
void SetDceKi(int32_t _val);
|
void SetDceKd(int32_t _val);
|
||||||
void SetDceKd(int32_t _val);
|
void ApplyPositionAsHome();
|
||||||
void ApplyPositionAsHome();
|
void SetEnableOnBoot(bool _enable);
|
||||||
void SetEnableOnBoot(bool _enable);
|
void SetEnableStallProtect(bool _enable);
|
||||||
void SetEnableAck(bool _enable);
|
void Reboot();
|
||||||
void SetEnableStallProtect(bool _enable);
|
void EraseConfigs();
|
||||||
void Reboot();
|
|
||||||
void EraseConfigs();
|
void UpdateAngle();
|
||||||
|
void UpdateAngleCallback(float _pos, bool _isFinished);
|
||||||
void UpdateAngle();
|
|
||||||
void UpdateAngleCallback(float _pos, bool _isAck);
|
|
||||||
|
// Communication protocol definitions
|
||||||
|
auto MakeProtocolDefinitions()
|
||||||
// Communication protocol definitions
|
{
|
||||||
auto MakeProtocolDefinitions()
|
return make_protocol_member_list(
|
||||||
{
|
make_protocol_ro_property("angle", &angle),
|
||||||
return make_protocol_member_list(
|
make_protocol_function("reboot", *this, &CtrlStepMotor::Reboot),
|
||||||
make_protocol_ro_property("angle", &angle),
|
make_protocol_function("erase_configs", *this, &CtrlStepMotor::EraseConfigs),
|
||||||
make_protocol_function("reboot", *this, &CtrlStepMotor::Reboot),
|
make_protocol_function("set_enable", *this, &CtrlStepMotor::SetEnable, "enable"),
|
||||||
make_protocol_function("erase_configs", *this, &CtrlStepMotor::EraseConfigs),
|
make_protocol_function("set_position_with_time", *this,
|
||||||
make_protocol_function("set_enable", *this, &CtrlStepMotor::SetEnable, "enable"),
|
&CtrlStepMotor::SetPositionWithVelocityLimit, "pos", "time"),
|
||||||
make_protocol_function("set_position_with_time", *this, &CtrlStepMotor::SetPositionWithTime,
|
make_protocol_function("set_position", *this, &CtrlStepMotor::SetPositionSetPoint, "pos"),
|
||||||
"pos", "time"),
|
make_protocol_function("set_velocity", *this, &CtrlStepMotor::SetVelocitySetPoint, "vel"),
|
||||||
make_protocol_function("set_position", *this, &CtrlStepMotor::SetPositionSetPoint, "pos"),
|
make_protocol_function("set_velocity_limit", *this, &CtrlStepMotor::SetVelocityLimit, "vel"),
|
||||||
make_protocol_function("set_velocity", *this, &CtrlStepMotor::SetVelocitySetPoint, "vel"),
|
make_protocol_function("set_current", *this, &CtrlStepMotor::SetCurrentSetPoint, "current"),
|
||||||
make_protocol_function("set_velocity_limit", *this, &CtrlStepMotor::SetVelocityLimit, "vel"),
|
make_protocol_function("set_current_limit", *this, &CtrlStepMotor::SetCurrentLimit, "current"),
|
||||||
make_protocol_function("set_current", *this, &CtrlStepMotor::SetCurrentSetPoint, "current"),
|
make_protocol_function("set_node_id", *this, &CtrlStepMotor::SetNodeID, "id"),
|
||||||
make_protocol_function("set_current_limit", *this, &CtrlStepMotor::SetCurrentLimit, "current"),
|
make_protocol_function("set_acceleration", *this, &CtrlStepMotor::SetAcceleration, "acc"),
|
||||||
make_protocol_function("set_node_id", *this, &CtrlStepMotor::SetNodeID, "id"),
|
make_protocol_function("apply_home_offset", *this, &CtrlStepMotor::ApplyPositionAsHome),
|
||||||
make_protocol_function("set_acceleration", *this, &CtrlStepMotor::SetAcceleration, "acc",
|
make_protocol_function("do_calibration", *this, &CtrlStepMotor::DoCalibration),
|
||||||
"stored"),
|
make_protocol_function("set_enable_on_boot", *this, &CtrlStepMotor::SetEnableOnBoot, "enable"),
|
||||||
make_protocol_function("apply_home_offset", *this, &CtrlStepMotor::ApplyPositionAsHome),
|
make_protocol_function("set_dce_kp", *this, &CtrlStepMotor::SetDceKp, "vel"),
|
||||||
make_protocol_function("do_calibration", *this, &CtrlStepMotor::DoCalibration),
|
make_protocol_function("set_dce_kv", *this, &CtrlStepMotor::SetDceKv, "vel"),
|
||||||
make_protocol_function("set_enable_on_boot", *this, &CtrlStepMotor::SetEnableOnBoot, "enable"),
|
make_protocol_function("set_dce_ki", *this, &CtrlStepMotor::SetDceKi, "vel"),
|
||||||
make_protocol_function("set_dce_kp", *this, &CtrlStepMotor::SetDceKp, "vel"),
|
make_protocol_function("set_dce_kd", *this, &CtrlStepMotor::SetDceKd, "vel"),
|
||||||
make_protocol_function("set_dce_kv", *this, &CtrlStepMotor::SetDceKv, "vel"),
|
make_protocol_function("set_enable_stall_protect", *this, &CtrlStepMotor::SetEnableStallProtect,
|
||||||
make_protocol_function("set_dce_ki", *this, &CtrlStepMotor::SetDceKi, "vel"),
|
"enable"),
|
||||||
make_protocol_function("set_dce_kd", *this, &CtrlStepMotor::SetDceKd, "vel"),
|
make_protocol_function("update_angle", *this, &CtrlStepMotor::UpdateAngle)
|
||||||
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 = {};
|
||||||
private:
|
};
|
||||||
CAN_HandleTypeDef* hcan;
|
|
||||||
uint8_t canBuf[8] = {};
|
#endif //DUMMY_CORE_FW_CTRL_STEP_HPP
|
||||||
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
|
#ifndef DOF6_KINEMATIC_SOLVER_H
|
||||||
#define DOF6_KINEMATIC_SOLVER_H
|
#define DOF6_KINEMATIC_SOLVER_H
|
||||||
|
|
||||||
#include "stm32f405xx.h"
|
#include "stm32f405xx.h"
|
||||||
#include "arm_math.h"
|
#include "arm_math.h"
|
||||||
#include "memory.h"
|
#include "memory.h"
|
||||||
|
|
||||||
class DOF6Kinematic
|
class DOF6Kinematic
|
||||||
{
|
{
|
||||||
private:
|
private:
|
||||||
const float RAD_TO_DEG = 57.295777754771045f;
|
const float RAD_TO_DEG = 57.295777754771045f;
|
||||||
|
|
||||||
// DH parameters
|
// DH parameters
|
||||||
struct ArmConfig_t
|
struct ArmConfig_t
|
||||||
{
|
{
|
||||||
float L_BASE;
|
float L_BASE;
|
||||||
float D_BASE;
|
float D_BASE;
|
||||||
float L_ARM;
|
float L_ARM;
|
||||||
float L_FOREARM;
|
float L_FOREARM;
|
||||||
float D_ELBOW;
|
float D_ELBOW;
|
||||||
float L_WRIST;
|
float L_WRIST;
|
||||||
};
|
};
|
||||||
ArmConfig_t armConfig;
|
ArmConfig_t armConfig;
|
||||||
|
|
||||||
float DH_matrix[6][4] = {0}; // home,d,a,alpha
|
float DH_matrix[6][4] = {0}; // home,d,a,alpha
|
||||||
float L1_base[3] = {0};
|
float L1_base[3] = {0};
|
||||||
float L2_arm[3] = {0};
|
float L2_arm[3] = {0};
|
||||||
float L3_elbow[3] = {0};
|
float L3_elbow[3] = {0};
|
||||||
float L6_wrist[3] = {0};
|
float L6_wrist[3] = {0};
|
||||||
|
|
||||||
float l_se_2;
|
float l_se_2;
|
||||||
float l_se;
|
float l_se;
|
||||||
float l_ew_2;
|
float l_ew_2;
|
||||||
float l_ew;
|
float l_ew;
|
||||||
float atan_e;
|
float atan_e;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
struct Joint6D_t
|
struct Joint6D_t
|
||||||
{
|
{
|
||||||
Joint6D_t()
|
Joint6D_t()
|
||||||
= default;
|
= default;
|
||||||
|
|
||||||
Joint6D_t(float a1, float a2, float a3, float a4, float a5, float a6)
|
Joint6D_t(float a1, float a2, float a3, float a4, float a5, float a6)
|
||||||
: a{a1, a2, a3, a4, a5, a6}
|
: a{a1, a2, a3, a4, a5, a6}
|
||||||
{}
|
{}
|
||||||
|
|
||||||
float a[6];
|
float a[6];
|
||||||
|
|
||||||
friend Joint6D_t operator-(const Joint6D_t &_joints1, const Joint6D_t &_joints2);
|
friend Joint6D_t operator-(const Joint6D_t &_joints1, const Joint6D_t &_joints2);
|
||||||
};
|
};
|
||||||
|
|
||||||
struct Pose6D_t
|
struct Pose6D_t
|
||||||
{
|
{
|
||||||
Pose6D_t()
|
Pose6D_t()
|
||||||
= default;
|
= default;
|
||||||
|
|
||||||
Pose6D_t(float x, float y, float z, float a, float b, float c)
|
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)
|
: X(x), Y(y), Z(z), A(a), B(b), C(c), hasR(false)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
float X{}, Y{}, Z{};
|
float X{}, Y{}, Z{};
|
||||||
float A{}, B{}, C{};
|
float A{}, B{}, C{};
|
||||||
float R[9]{};
|
float R[9]{};
|
||||||
|
|
||||||
// if Pose was calculated by FK then it's true automatically (so that no need to do extra calc),
|
// 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.
|
// otherwise if manually set params then it should be set to false.
|
||||||
bool hasR{};
|
bool hasR{};
|
||||||
};
|
};
|
||||||
|
|
||||||
struct IKSolves_t
|
struct IKSolves_t
|
||||||
{
|
{
|
||||||
Joint6D_t config[8];
|
Joint6D_t config[8];
|
||||||
char solFlag[8][3];
|
char solFlag[8][3];
|
||||||
};
|
};
|
||||||
|
|
||||||
DOF6Kinematic(float L_BS, float D_BS, float L_SE, float L_EW, float D_EW, float L_WT);
|
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 SolveFK(const Joint6D_t &_inputJoint6D, Pose6D_t &_outputPose6D);
|
||||||
|
|
||||||
bool SolveIK(const Pose6D_t &_inputPose6D, const Joint6D_t &_lastJoint6D, IKSolves_t &_outputSolves);
|
bool SolveIK(const Pose6D_t &_inputPose6D, const Joint6D_t &_lastJoint6D, IKSolves_t &_outputSolves);
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif //DOF6_KINEMATIC_SOLVER_H
|
#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
|
#ifndef REF_STM32F4_FW_DUMMY_ROBOT_H
|
||||||
#define REF_STM32F4_FW_DUMMY_ROBOT_H
|
#define REF_STM32F4_FW_DUMMY_ROBOT_H
|
||||||
|
|
||||||
#include "algorithms/kinematic/6dof_kinematic.h"
|
#include "algorithms/kinematic/6dof_kinematic.h"
|
||||||
#include "actuators/ctrl_step/ctrl_step.hpp"
|
#include "actuators/ctrl_step/ctrl_step.hpp"
|
||||||
|
|
||||||
#define ALL 0
|
#define ALL 0
|
||||||
|
|
||||||
/*
|
/*
|
||||||
| PARAMS | `current_limit` | `acceleration` | `dce_kp` | `dce_kv` | `dce_ki` | `dce_kd` |
|
| PARAMS | `current_limit` | `acceleration` | `dce_kp` | `dce_kv` | `dce_ki` | `dce_kd` |
|
||||||
| ---------- | --------------- | -------------- | -------- | -------- | -------- | -------- |
|
| ---------- | --------------- | -------------- | -------- | -------- | -------- | -------- |
|
||||||
| **Joint1** | 1 | 25 | 1000 | 80 | 300 | 250 |
|
| **Joint1** | 2 | 30 | 1000 | 80 | 200 | 250 |
|
||||||
| **Joint2** | 1.5 | 25 | 1000 | 250 | 300 | 200 |
|
| **Joint2** | 2 | 30 | 1000 | 80 | 200 | 200 |
|
||||||
| **Joint3** | 1 | 25 | 1000 | 350 | 500 | 250 |
|
| **Joint3** | 2 | 30 | 1500 | 80 | 200 | 250 |
|
||||||
| **Joint4** | 1.5 | 25 | 1000 | 350 | 500 | 250 |
|
| **Joint4** | 2 | 30 | 1000 | 80 | 200 | 250 |
|
||||||
| **Joint5** | 1.5 | 25 | 1000 | 350 | 500 | 250 |
|
| **Joint5** | 2 | 30 | 1000 | 80 | 200 | 250 |
|
||||||
| **Joint6** | 1.5 | 25 | 1000 | 350 | 500 | 250 |
|
| **Joint6** | 2 | 30 | 1000 | 80 | 200 | 250 |
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
class DummyHand
|
class DummyHand
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
uint8_t nodeID = 7;
|
uint8_t nodeID = 7;
|
||||||
float maxCurrent = 0.7;
|
float maxCurrent = 0.7;
|
||||||
|
|
||||||
|
|
||||||
DummyHand(CAN_HandleTypeDef* _hcan, uint8_t _id);
|
DummyHand(CAN_HandleTypeDef* _hcan, uint8_t _id);
|
||||||
|
|
||||||
|
|
||||||
void SetAngle(float _angle);
|
void SetAngle(float _angle);
|
||||||
void SetMaxCurrent(float _val);
|
void SetMaxCurrent(float _val);
|
||||||
void SetEnable(bool _enable);
|
void SetEnable(bool _enable);
|
||||||
|
|
||||||
|
|
||||||
// Communication protocol definitions
|
// Communication protocol definitions
|
||||||
auto MakeProtocolDefinitions()
|
auto MakeProtocolDefinitions()
|
||||||
{
|
{
|
||||||
return make_protocol_member_list(
|
return make_protocol_member_list(
|
||||||
make_protocol_function("set_angle", *this, &DummyHand::SetAngle, "angle"),
|
make_protocol_function("set_angle", *this, &DummyHand::SetAngle, "angle"),
|
||||||
make_protocol_function("set_enable", *this, &DummyHand::SetEnable, "enable"),
|
make_protocol_function("set_enable", *this, &DummyHand::SetEnable, "enable"),
|
||||||
make_protocol_function("set_current_limit", *this, &DummyHand::SetMaxCurrent, "current")
|
make_protocol_function("set_current_limit", *this, &DummyHand::SetMaxCurrent, "current")
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
CAN_HandleTypeDef* hcan;
|
CAN_HandleTypeDef* hcan;
|
||||||
uint8_t canBuf[8];
|
uint8_t canBuf[8];
|
||||||
CAN_TxHeaderTypeDef txHeader;
|
CAN_TxHeaderTypeDef txHeader;
|
||||||
float minAngle = 0;
|
float minAngle = 0;
|
||||||
float maxAngle = 45;
|
float maxAngle = 45;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
class DummyRobot
|
class DummyRobot
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
explicit DummyRobot(CAN_HandleTypeDef* _hcan);
|
explicit DummyRobot(CAN_HandleTypeDef* _hcan);
|
||||||
~DummyRobot();
|
~DummyRobot();
|
||||||
|
|
||||||
|
|
||||||
enum CommandMode
|
enum CommandMode
|
||||||
{
|
{
|
||||||
COMMAND_TARGET_POINT_SEQUENTIAL = 1,
|
COMMAND_TARGET_POINT_SEQUENTIAL = 1,
|
||||||
COMMAND_TARGET_POINT_INTERRUPTABLE,
|
COMMAND_TARGET_POINT_INTERRUPTABLE,
|
||||||
COMMAND_CONTINUES_TRAJECTORY
|
COMMAND_CONTINUES_TRAJECTORY,
|
||||||
};
|
COMMAND_MOTOR_TUNING
|
||||||
|
};
|
||||||
// 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;
|
class TuningHelper
|
||||||
const float DEFAULT_JOINT_ACCELERATION = 50;
|
{
|
||||||
|
public:
|
||||||
DOF6Kinematic::Joint6D_t currentJoints = REST_POSE;
|
explicit TuningHelper(DummyRobot* _context) : context(_context)
|
||||||
DOF6Kinematic::Joint6D_t initPose = REST_POSE;
|
{
|
||||||
DOF6Kinematic::Pose6D_t currentPose6D = {};
|
}
|
||||||
volatile uint8_t jointsStateFlag = 0b00000000;
|
|
||||||
CommandMode commandMode = COMMAND_TARGET_POINT_SEQUENTIAL;
|
void SetTuningFlag(uint8_t _flag);
|
||||||
bool isStopped = false;
|
void Tick(uint32_t _timeMillis);
|
||||||
CtrlStepMotor* motorJ[7] = {nullptr};
|
void SetFreqAndAmp(float _freq, float _amp);
|
||||||
DummyHand* hand = {nullptr};
|
|
||||||
|
|
||||||
|
// Communication protocol definitions
|
||||||
float MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6);
|
auto MakeProtocolDefinitions()
|
||||||
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);
|
return make_protocol_member_list(
|
||||||
void MoveTrajectoryL(float _x, float _y, float _z, float _a, float _b, float _c);
|
make_protocol_function("set_tuning_freq_amp", *this,
|
||||||
void SetJointSpeed(float _speed);
|
&TuningHelper::SetFreqAndAmp, "freq", "amp"),
|
||||||
void SetJointAcceleration(float _acc);
|
make_protocol_function("set_tuning_flag", *this,
|
||||||
void UpdateJointAngles();
|
&TuningHelper::SetTuningFlag, "flag")
|
||||||
void UpdateJointAnglesCallback();
|
);
|
||||||
void UpdateJointPose6D();
|
}
|
||||||
void Reboot();
|
|
||||||
void SetEnable(bool _enable);
|
|
||||||
void CalibrateHomeOffset();
|
private:
|
||||||
void Homing();
|
DummyRobot* context;
|
||||||
void Resting();
|
float time = 0;
|
||||||
bool IsMoving();
|
uint8_t tuningFlag = 0;
|
||||||
void SetCommandMode(uint8_t _mode);
|
float frequency = 1;
|
||||||
|
float amplitude = 1;
|
||||||
|
};
|
||||||
// Communication protocol definitions
|
TuningHelper tuningHelper = TuningHelper(this);
|
||||||
auto MakeProtocolDefinitions()
|
|
||||||
{
|
|
||||||
return make_protocol_member_list(
|
// This is the pose when power on.
|
||||||
make_protocol_function("calibrate_home_offset", *this, &DummyRobot::CalibrateHomeOffset),
|
const DOF6Kinematic::Joint6D_t REST_POSE = {0, -73, 180, 0, 0, 0};
|
||||||
make_protocol_function("homing", *this, &DummyRobot::Homing),
|
const float DEFAULT_JOINT_SPEED = 30; // degree/s
|
||||||
make_protocol_function("resting", *this, &DummyRobot::Resting),
|
const DOF6Kinematic::Joint6D_t DEFAULT_JOINT_ACCELERATION_BASES = {150, 100, 200, 200, 200, 200};
|
||||||
make_protocol_object("joint_1", motorJ[1]->MakeProtocolDefinitions()),
|
const float DEFAULT_JOINT_ACCELERATION_LOW = 30; // 0~100
|
||||||
make_protocol_object("joint_2", motorJ[2]->MakeProtocolDefinitions()),
|
const float DEFAULT_JOINT_ACCELERATION_HIGH = 100; // 0~100
|
||||||
make_protocol_object("joint_3", motorJ[3]->MakeProtocolDefinitions()),
|
const CommandMode DEFAULT_COMMAND_MODE = COMMAND_TARGET_POINT_INTERRUPTABLE;
|
||||||
make_protocol_object("joint_4", motorJ[4]->MakeProtocolDefinitions()),
|
|
||||||
make_protocol_object("joint_5", motorJ[5]->MakeProtocolDefinitions()),
|
|
||||||
make_protocol_object("joint_6", motorJ[6]->MakeProtocolDefinitions()),
|
DOF6Kinematic::Joint6D_t currentJoints = REST_POSE;
|
||||||
make_protocol_object("joint_all", motorJ[ALL]->MakeProtocolDefinitions()),
|
DOF6Kinematic::Joint6D_t targetJoints = REST_POSE;
|
||||||
make_protocol_object("hand", hand->MakeProtocolDefinitions()),
|
DOF6Kinematic::Joint6D_t initPose = REST_POSE;
|
||||||
make_protocol_function("reboot", *this, &DummyRobot::Reboot),
|
DOF6Kinematic::Pose6D_t currentPose6D = {};
|
||||||
make_protocol_function("set_enable", *this, &DummyRobot::SetEnable, "enable"),
|
volatile uint8_t jointsStateFlag = 0b00000000;
|
||||||
make_protocol_function("move_j", *this, &DummyRobot::MoveJ, "j1", "j2", "j3", "j4", "j5", "j6"),
|
CommandMode commandMode = DEFAULT_COMMAND_MODE;
|
||||||
make_protocol_function("move_l", *this, &DummyRobot::MoveL, "x", "y", "z", "a", "b", "c"),
|
CtrlStepMotor* motorJ[7] = {nullptr};
|
||||||
make_protocol_function("set_joint_speed", *this, &DummyRobot::SetJointSpeed, "speed"),
|
DummyHand* hand = {nullptr};
|
||||||
make_protocol_function("set_joint_acc", *this, &DummyRobot::SetJointAcceleration, "acc"),
|
|
||||||
make_protocol_function("set_command_mode", *this, &DummyRobot::SetCommandMode, "mode")
|
|
||||||
|
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);
|
||||||
class CommandHandler
|
void SetJointAcceleration(float _acc);
|
||||||
{
|
void UpdateJointAngles();
|
||||||
public:
|
void UpdateJointAnglesCallback();
|
||||||
explicit CommandHandler(DummyRobot* _context) : context(_context)
|
void UpdateJointPose6D();
|
||||||
{
|
void Reboot();
|
||||||
commandFifo = osMessageQueueNew(16, 64, nullptr);
|
void SetEnable(bool _enable);
|
||||||
commandLifo = osMessageQueueNew(1, 64, nullptr);
|
void CalibrateHomeOffset();
|
||||||
}
|
void Homing();
|
||||||
|
void Resting();
|
||||||
uint32_t Push(const std::string &_cmd);
|
bool IsMoving();
|
||||||
std::string Pop(uint32_t timeout);
|
bool IsEnabled();
|
||||||
uint32_t ParseCommand(const std::string &_cmd);
|
void SetCommandMode(uint32_t _mode);
|
||||||
uint32_t GetSpace();
|
|
||||||
void ClearFifo();
|
|
||||||
void EmergencyStop();
|
// Communication protocol definitions
|
||||||
void Resume();
|
auto MakeProtocolDefinitions()
|
||||||
|
{
|
||||||
|
return make_protocol_member_list(
|
||||||
private:
|
make_protocol_function("calibrate_home_offset", *this, &DummyRobot::CalibrateHomeOffset),
|
||||||
DummyRobot* context;
|
make_protocol_function("homing", *this, &DummyRobot::Homing),
|
||||||
osMessageQueueId_t commandFifo;
|
make_protocol_function("resting", *this, &DummyRobot::Resting),
|
||||||
osMessageQueueId_t commandLifo;
|
make_protocol_object("joint_1", motorJ[1]->MakeProtocolDefinitions()),
|
||||||
char strBuffer[64]{};
|
make_protocol_object("joint_2", motorJ[2]->MakeProtocolDefinitions()),
|
||||||
};
|
make_protocol_object("joint_3", motorJ[3]->MakeProtocolDefinitions()),
|
||||||
CommandHandler commandHandler = CommandHandler(this);
|
make_protocol_object("joint_4", motorJ[4]->MakeProtocolDefinitions()),
|
||||||
|
make_protocol_object("joint_5", motorJ[5]->MakeProtocolDefinitions()),
|
||||||
|
make_protocol_object("joint_6", motorJ[6]->MakeProtocolDefinitions()),
|
||||||
private:
|
make_protocol_object("joint_all", motorJ[ALL]->MakeProtocolDefinitions()),
|
||||||
CAN_HandleTypeDef* hcan;
|
make_protocol_object("hand", hand->MakeProtocolDefinitions()),
|
||||||
float jointSpeed = DEFAULT_JOINT_SPEED;
|
make_protocol_function("reboot", *this, &DummyRobot::Reboot),
|
||||||
DOF6Kinematic* dof6Solver;
|
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"),
|
||||||
float MoveJoints(DOF6Kinematic::Joint6D_t _joints);
|
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())
|
||||||
#endif //REF_STM32F4_FW_DUMMY_ROBOT_H
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
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"
|
#include "common_inc.h"
|
||||||
|
|
||||||
|
|
||||||
// On-board Screen, can choose from hi2c2 or hi2c0(soft i2c)
|
// On-board Screen, can choose from hi2c2 or hi2c0(soft i2c)
|
||||||
SSD1306 oled(&hi2c0);
|
SSD1306 oled(&hi2c0);
|
||||||
// On-board Sensor, used hi2c1
|
// On-board Sensor, used hi2c1
|
||||||
MPU6050 mpu6050(&hi2c1);
|
MPU6050 mpu6050(&hi2c1);
|
||||||
// 5 User-Timers, can choose from htim7/htim10/htim11/htim13/htim14
|
// 5 User-Timers, can choose from htim7/htim10/htim11/htim13/htim14
|
||||||
Timer timerCtrlLoop(&htim7, 200);
|
Timer timerCtrlLoop(&htim7, 200);
|
||||||
// 2x2-channel PWMs, used htim9 & htim12, each has 2-channel outputs
|
// 2x2-channel PWMs, used htim9 & htim12, each has 2-channel outputs
|
||||||
PWM pwm(21000, 21000);
|
PWM pwm(21000, 21000);
|
||||||
// Robot instance
|
// Robot instance
|
||||||
DummyRobot dummy(&hcan1);
|
DummyRobot dummy(&hcan1);
|
||||||
|
|
||||||
|
|
||||||
/* Thread Definitions -----------------------------------------------------*/
|
/* Thread Definitions -----------------------------------------------------*/
|
||||||
osThreadId_t controlLoopFixUpdateHandle;
|
osThreadId_t controlLoopFixUpdateHandle;
|
||||||
void ThreadControlLoopFixUpdate(void* argument)
|
void ThreadControlLoopFixUpdate(void* argument)
|
||||||
{
|
{
|
||||||
for (;;)
|
for (;;)
|
||||||
{
|
{
|
||||||
// Suspended here until got Notification.
|
// Suspended here until got Notification.
|
||||||
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
|
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
|
||||||
|
|
||||||
switch (dummy.commandMode)
|
if (dummy.IsEnabled())
|
||||||
{
|
{
|
||||||
case DummyRobot::COMMAND_TARGET_POINT_SEQUENTIAL:
|
// Send control command to Motors & update Joint states
|
||||||
case DummyRobot::COMMAND_TARGET_POINT_INTERRUPTABLE:
|
switch (dummy.commandMode)
|
||||||
dummy.UpdateJointAngles();
|
{
|
||||||
dummy.UpdateJointPose6D();
|
case DummyRobot::COMMAND_TARGET_POINT_SEQUENTIAL:
|
||||||
break;
|
case DummyRobot::COMMAND_TARGET_POINT_INTERRUPTABLE:
|
||||||
case DummyRobot::COMMAND_CONTINUES_TRAJECTORY:
|
case DummyRobot::COMMAND_CONTINUES_TRAJECTORY:
|
||||||
// ToDo: handle trajectory, while update state at the mean time
|
dummy.MoveJoints(dummy.targetJoints);
|
||||||
dummy.UpdateJointPose6D();
|
dummy.UpdateJointPose6D();
|
||||||
break;
|
break;
|
||||||
}
|
case DummyRobot::COMMAND_MOTOR_TUNING:
|
||||||
}
|
dummy.tuningHelper.Tick(10);
|
||||||
}
|
dummy.UpdateJointPose6D();
|
||||||
|
break;
|
||||||
|
}
|
||||||
osThreadId_t ControlLoopUpdateHandle;
|
} else
|
||||||
void ThreadControlLoopUpdate(void* argument)
|
{
|
||||||
{
|
// Just update Joint states
|
||||||
for (;;)
|
dummy.UpdateJointAngles();
|
||||||
{
|
dummy.UpdateJointPose6D();
|
||||||
dummy.commandHandler.ParseCommand(dummy.commandHandler.Pop(osWaitForever));
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
osThreadId_t oledTaskHandle;
|
osThreadId_t ControlLoopUpdateHandle;
|
||||||
void ThreadOledUpdate(void* argument)
|
void ThreadControlLoopUpdate(void* argument)
|
||||||
{
|
{
|
||||||
uint32_t t = micros();
|
for (;;)
|
||||||
char buf[16];
|
{
|
||||||
char cmdModeNames[3][4] = {"SEQ", "INT", "TRJ"};
|
dummy.commandHandler.ParseCommand(dummy.commandHandler.Pop(osWaitForever));
|
||||||
|
}
|
||||||
for (;;)
|
}
|
||||||
{
|
|
||||||
mpu6050.Update(true);
|
|
||||||
|
osThreadId_t oledTaskHandle;
|
||||||
oled.clearBuffer();
|
void ThreadOledUpdate(void* argument)
|
||||||
|
{
|
||||||
oled.setFont(u8g2_font_5x8_tr);
|
uint32_t t = micros();
|
||||||
oled.setCursor(0, 10);
|
char buf[16];
|
||||||
oled.printf("IMU:%.3f/%.3f", mpu6050.data.ax, mpu6050.data.ay);
|
char cmdModeNames[4][4] = {"SEQ", "INT", "TRJ", "TUN"};
|
||||||
oled.setCursor(85, 10);
|
|
||||||
oled.printf("| FPS:%lu", 1000000 / (micros() - t));
|
for (;;)
|
||||||
t = micros();
|
{
|
||||||
|
mpu6050.Update(true);
|
||||||
oled.drawBox(0, 15, 128, 3);
|
|
||||||
oled.setCursor(0, 30);
|
oled.clearBuffer();
|
||||||
oled.printf(">%3d|%3d|%3d|%3d|%3d|%3d",
|
oled.setFont(u8g2_font_5x8_tr);
|
||||||
(int) roundf(dummy.currentJoints.a[0]), (int) roundf(dummy.currentJoints.a[1]),
|
oled.setCursor(0, 10);
|
||||||
(int) roundf(dummy.currentJoints.a[2]), (int) roundf(dummy.currentJoints.a[3]),
|
oled.printf("IMU:%.3f/%.3f", mpu6050.data.ax, mpu6050.data.ay);
|
||||||
(int) roundf(dummy.currentJoints.a[4]), (int) roundf(dummy.currentJoints.a[5]));
|
oled.setCursor(85, 10);
|
||||||
|
oled.printf("| FPS:%lu", 1000000 / (micros() - t));
|
||||||
oled.drawBox(40, 35, 128, 24);
|
t = micros();
|
||||||
oled.setFont(u8g2_font_6x12_tr);
|
|
||||||
oled.setDrawColor(0);
|
oled.drawBox(0, 15, 128, 3);
|
||||||
oled.setCursor(42, 45);
|
oled.setCursor(0, 30);
|
||||||
oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.X),
|
oled.printf(">%3d|%3d|%3d|%3d|%3d|%3d",
|
||||||
(int) roundf(dummy.currentPose6D.Y), (int) roundf(dummy.currentPose6D.Z));
|
(int) roundf(dummy.currentJoints.a[0]), (int) roundf(dummy.currentJoints.a[1]),
|
||||||
oled.setCursor(42, 56);
|
(int) roundf(dummy.currentJoints.a[2]), (int) roundf(dummy.currentJoints.a[3]),
|
||||||
oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.A),
|
(int) roundf(dummy.currentJoints.a[4]), (int) roundf(dummy.currentJoints.a[5]));
|
||||||
(int) roundf(dummy.currentPose6D.B), (int) roundf(dummy.currentPose6D.C));
|
|
||||||
oled.setDrawColor(1);
|
oled.drawBox(40, 35, 128, 24);
|
||||||
oled.setCursor(0, 45);
|
oled.setFont(u8g2_font_6x12_tr);
|
||||||
oled.printf("[XYZ]:");
|
oled.setDrawColor(0);
|
||||||
oled.setCursor(0, 56);
|
oled.setCursor(42, 45);
|
||||||
oled.printf("[ABC]:");
|
oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.X),
|
||||||
|
(int) roundf(dummy.currentPose6D.Y), (int) roundf(dummy.currentPose6D.Z));
|
||||||
oled.setFont(u8g2_font_10x20_tr);
|
oled.setCursor(42, 56);
|
||||||
oled.setCursor(0, 78);
|
oled.printf("%4d|%4d|%4d", (int) roundf(dummy.currentPose6D.A),
|
||||||
for (int i = 1; i <= 6; i++)
|
(int) roundf(dummy.currentPose6D.B), (int) roundf(dummy.currentPose6D.C));
|
||||||
buf[i - 1] = (dummy.jointsStateFlag & (1 << i) ? '*' : '_');
|
oled.setDrawColor(1);
|
||||||
buf[6] = 0;
|
oled.setCursor(0, 45);
|
||||||
oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], buf);
|
oled.printf("[XYZ]:");
|
||||||
|
oled.setCursor(0, 56);
|
||||||
oled.sendBuffer();
|
oled.printf("[ABC]:");
|
||||||
}
|
|
||||||
}
|
oled.setFont(u8g2_font_10x20_tr);
|
||||||
|
oled.setCursor(0, 78);
|
||||||
|
if (dummy.IsEnabled())
|
||||||
/* Timer Callbacks -------------------------------------------------------*/
|
{
|
||||||
void OnTimer7Callback()
|
for (int i = 1; i <= 6; i++)
|
||||||
{
|
buf[i - 1] = (dummy.jointsStateFlag & (1 << i) ? '*' : '_');
|
||||||
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
buf[6] = 0;
|
||||||
|
oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], buf);
|
||||||
// Wake & invoke thread IMMEDIATELY.
|
} else
|
||||||
vTaskNotifyGiveFromISR(TaskHandle_t(controlLoopFixUpdateHandle), &xHigherPriorityTaskWoken);
|
{
|
||||||
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], "======");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
oled.sendBuffer();
|
||||||
/* Default Entry -------------------------------------------------------*/
|
}
|
||||||
void Main(void)
|
}
|
||||||
{
|
|
||||||
// Init all communication staff, include USB-CDC/VCP/UART/CAN etc.
|
|
||||||
InitCommunication();
|
/* Timer Callbacks -------------------------------------------------------*/
|
||||||
|
void OnTimer7Callback()
|
||||||
// Init IMU.
|
{
|
||||||
do
|
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
|
||||||
{
|
|
||||||
mpu6050.Init();
|
// Wake & invoke thread IMMEDIATELY.
|
||||||
osDelay(100);
|
vTaskNotifyGiveFromISR(TaskHandle_t(controlLoopFixUpdateHandle), &xHigherPriorityTaskWoken);
|
||||||
} while (!mpu6050.testConnection());
|
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
|
||||||
mpu6050.InitFilter(200, 100, 50);
|
}
|
||||||
|
|
||||||
// Init OLED 128x80.
|
|
||||||
oled.Init();
|
/* Default Entry -------------------------------------------------------*/
|
||||||
pwm.Start();
|
void Main(void)
|
||||||
|
{
|
||||||
// Init & Run User Threads.
|
// Init all communication staff, including USB-CDC/VCP/UART/CAN etc.
|
||||||
const osThreadAttr_t controlLoopTask_attributes = {
|
InitCommunication();
|
||||||
.name = "ControlLoopFixUpdateTask",
|
|
||||||
.stack_size = 1000 * 4,
|
// Init Robot.
|
||||||
.priority = (osPriority_t) osPriorityRealtime,
|
dummy.Init();
|
||||||
};
|
|
||||||
controlLoopFixUpdateHandle = osThreadNew(ThreadControlLoopFixUpdate, nullptr,
|
// Init IMU.
|
||||||
&controlLoopTask_attributes);
|
do
|
||||||
|
{
|
||||||
const osThreadAttr_t ControlLoopUpdateTask_attributes = {
|
mpu6050.Init();
|
||||||
.name = "ControlLoopUpdateTask",
|
osDelay(100);
|
||||||
.stack_size = 1000 * 4,
|
} while (!mpu6050.testConnection());
|
||||||
.priority = (osPriority_t) osPriorityNormal,
|
mpu6050.InitFilter(200, 100, 50);
|
||||||
};
|
|
||||||
ControlLoopUpdateHandle = osThreadNew(ThreadControlLoopUpdate, nullptr,
|
// Init OLED 128x80.
|
||||||
&ControlLoopUpdateTask_attributes);
|
oled.Init();
|
||||||
|
pwm.Start();
|
||||||
const osThreadAttr_t oledTask_attributes = {
|
|
||||||
.name = "OledTask",
|
// Init & Run User Threads.
|
||||||
.stack_size = 1000 * 4,
|
const osThreadAttr_t controlLoopTask_attributes = {
|
||||||
.priority = (osPriority_t) osPriorityNormal, // should >= Normal
|
.name = "ControlLoopFixUpdateTask",
|
||||||
};
|
.stack_size = 2000,
|
||||||
oledTaskHandle = osThreadNew(ThreadOledUpdate, nullptr, &oledTask_attributes);
|
.priority = (osPriority_t) osPriorityRealtime,
|
||||||
|
};
|
||||||
// Start Timer Callbacks.
|
controlLoopFixUpdateHandle = osThreadNew(ThreadControlLoopFixUpdate, nullptr,
|
||||||
timerCtrlLoop.SetCallback(OnTimer7Callback);
|
&controlLoopTask_attributes);
|
||||||
timerCtrlLoop.Start();
|
|
||||||
|
const osThreadAttr_t ControlLoopUpdateTask_attributes = {
|
||||||
// System started, light switch-led up.
|
.name = "ControlLoopUpdateTask",
|
||||||
pwm.SetDuty(PWM::CH_A1, 0.5);
|
.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"
|
#include "common_inc.h"
|
||||||
|
|
||||||
extern DummyRobot dummy;
|
extern DummyRobot dummy;
|
||||||
|
|
||||||
|
|
||||||
void OnUsbAsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
|
void OnUsbAsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
|
||||||
{
|
{
|
||||||
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
|
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
|
||||||
if (_cmd[0] == '!' || dummy.isStopped)
|
if (_cmd[0] == '!' || !dummy.IsEnabled())
|
||||||
{
|
{
|
||||||
std::string s(_cmd);
|
std::string s(_cmd);
|
||||||
if (s.find("STOP") != std::string::npos)
|
if (s.find("STOP") != std::string::npos)
|
||||||
{
|
{
|
||||||
dummy.commandHandler.EmergencyStop();
|
dummy.commandHandler.EmergencyStop();
|
||||||
Respond(_responseChannel, "!!!Stopped!!!");
|
Respond(_responseChannel, "Stopped ok");
|
||||||
} else if (s.find("RESUME") != std::string::npos)
|
} else if (s.find("START") != std::string::npos)
|
||||||
{
|
{
|
||||||
dummy.commandHandler.Resume();
|
dummy.SetEnable(true);
|
||||||
Respond(_responseChannel, "Resumed");
|
Respond(_responseChannel, "Started ok");
|
||||||
}
|
} else if (s.find("DISABLE") != std::string::npos)
|
||||||
} else if (_cmd[0] == '#')
|
{
|
||||||
{
|
dummy.SetEnable(false);
|
||||||
std::string s(_cmd);
|
Respond(_responseChannel, "Disabled ok");
|
||||||
if (s.find("GETJPOS") != std::string::npos)
|
}
|
||||||
{
|
} else if (_cmd[0] == '#')
|
||||||
Respond(*usbStreamOutputPtr, "JNTS %.2f %.2f %.2f %.2f %.2f %.2f",
|
{
|
||||||
dummy.currentJoints.a[0], dummy.currentJoints.a[1],
|
std::string s(_cmd);
|
||||||
dummy.currentJoints.a[2], dummy.currentJoints.a[3],
|
if (s.find("GETJPOS") != std::string::npos)
|
||||||
dummy.currentJoints.a[4], dummy.currentJoints.a[5]);
|
{
|
||||||
} else if (s.find("GETLPOS") != std::string::npos)
|
Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
|
||||||
{
|
dummy.currentJoints.a[0], dummy.currentJoints.a[1],
|
||||||
dummy.UpdateJointPose6D();
|
dummy.currentJoints.a[2], dummy.currentJoints.a[3],
|
||||||
Respond(*usbStreamOutputPtr, "POSE %.2f %.2f %.2f %.2f %.2f %.2f",
|
dummy.currentJoints.a[4], dummy.currentJoints.a[5]);
|
||||||
dummy.currentPose6D.X, dummy.currentPose6D.Y,
|
} else if (s.find("GETLPOS") != std::string::npos)
|
||||||
dummy.currentPose6D.Z, dummy.currentPose6D.A,
|
{
|
||||||
dummy.currentPose6D.B, dummy.currentPose6D.C);
|
dummy.UpdateJointPose6D();
|
||||||
} else if (s.find("CMDMODE") != std::string::npos)
|
Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
|
||||||
{
|
dummy.currentPose6D.X, dummy.currentPose6D.Y,
|
||||||
int mode;
|
dummy.currentPose6D.Z, dummy.currentPose6D.A,
|
||||||
sscanf(_cmd, "CMDMODE %d", &mode);
|
dummy.currentPose6D.B, dummy.currentPose6D.C);
|
||||||
dummy.SetCommandMode(mode);
|
} else if (s.find("CMDMODE") != std::string::npos)
|
||||||
Respond(*usbStreamOutputPtr, "Set command mode to [%d]", mode);
|
{
|
||||||
} else
|
uint32_t mode;
|
||||||
Respond(*usbStreamOutputPtr, "ok");
|
sscanf(_cmd, "#CMDMODE %lu", &mode);
|
||||||
} else if (_cmd[0] == '>' || _cmd[0] == '@')
|
dummy.SetCommandMode(mode);
|
||||||
{
|
Respond(_responseChannel, "Set command mode to [%lu]", mode);
|
||||||
uint32_t freeSize = dummy.commandHandler.Push(_cmd);
|
} else
|
||||||
Respond(_responseChannel, "%d", freeSize);
|
Respond(_responseChannel, "ok");
|
||||||
}
|
} else if (_cmd[0] == '>' || _cmd[0] == '@')
|
||||||
|
{
|
||||||
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
|
uint32_t freeSize = dummy.commandHandler.Push(_cmd);
|
||||||
}
|
Respond(_responseChannel, "%d", freeSize);
|
||||||
|
}
|
||||||
|
|
||||||
void OnUart4AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
|
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
|
||||||
{
|
}
|
||||||
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
|
|
||||||
uint8_t argNum;
|
|
||||||
|
void OnUart4AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
|
||||||
if (_cmd[0] == '#')
|
{
|
||||||
{
|
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
|
||||||
std::string s(_cmd);
|
if (_cmd[0] == '!' || !dummy.IsEnabled())
|
||||||
if (s.find("GETJPOS") != std::string::npos)
|
{
|
||||||
{
|
std::string s(_cmd);
|
||||||
Respond(_responseChannel, "JNTS %.2f %.2f %.2f %.2f %.2f %.2f",
|
if (s.find("STOP") != std::string::npos)
|
||||||
dummy.currentJoints.a[0], dummy.currentJoints.a[1], dummy.currentJoints.a[2],
|
{
|
||||||
dummy.currentJoints.a[3], dummy.currentJoints.a[4], dummy.currentJoints.a[5]);
|
dummy.commandHandler.EmergencyStop();
|
||||||
} else
|
Respond(_responseChannel, "Stopped ok");
|
||||||
Respond(_responseChannel, "ok");
|
} else if (s.find("START") != std::string::npos)
|
||||||
} else if (_cmd[0] == '>')
|
{
|
||||||
{
|
dummy.SetEnable(true);
|
||||||
float joints[6];
|
Respond(_responseChannel, "Started ok");
|
||||||
float speed;
|
} else if (s.find("DISABLE") != std::string::npos)
|
||||||
|
{
|
||||||
argNum = sscanf(_cmd, ">%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2,
|
dummy.SetEnable(false);
|
||||||
joints + 3, joints + 4, joints + 5, &speed);
|
Respond(_responseChannel, "Disabled ok");
|
||||||
if (argNum == 6)
|
}
|
||||||
{
|
} else if (_cmd[0] == '#')
|
||||||
dummy.MoveJ(joints[0], joints[1], joints[2],
|
{
|
||||||
joints[3], joints[4], joints[5]);
|
std::string s(_cmd);
|
||||||
} else if (argNum == 7)
|
if (s.find("GETJPOS") != std::string::npos)
|
||||||
{
|
{
|
||||||
dummy.SetJointSpeed(speed);
|
Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
|
||||||
dummy.MoveJ(joints[0], joints[1], joints[2],
|
dummy.currentJoints.a[0], dummy.currentJoints.a[1],
|
||||||
joints[3], joints[4], joints[5]);
|
dummy.currentJoints.a[2], dummy.currentJoints.a[3],
|
||||||
}
|
dummy.currentJoints.a[4], dummy.currentJoints.a[5]);
|
||||||
|
} else if (s.find("GETLPOS") != std::string::npos)
|
||||||
while (dummy.IsMoving())
|
{
|
||||||
osDelay(10);
|
dummy.UpdateJointPose6D();
|
||||||
Respond(_responseChannel, "ok");
|
Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
|
||||||
|
dummy.currentPose6D.X, dummy.currentPose6D.Y,
|
||||||
} else if (_cmd[0] == '@')
|
dummy.currentPose6D.Z, dummy.currentPose6D.A,
|
||||||
{
|
dummy.currentPose6D.B, dummy.currentPose6D.C);
|
||||||
float pose[6];
|
} else if (s.find("CMDMODE") != std::string::npos)
|
||||||
float speed;
|
{
|
||||||
|
uint32_t mode;
|
||||||
argNum = sscanf(_cmd, "@%f,%f,%f,%f,%f,%f,%f", pose, pose + 1, pose + 2,
|
sscanf(_cmd, "#CMDMODE %lu", &mode);
|
||||||
pose + 3, pose + 4, pose + 5, &speed);
|
dummy.SetCommandMode(mode);
|
||||||
if (argNum == 6)
|
Respond(_responseChannel, "Set command mode to [%lu]", mode);
|
||||||
{
|
} else
|
||||||
dummy.MoveL(pose[0], pose[1], pose[2],
|
Respond(_responseChannel, "ok");
|
||||||
pose[3], pose[4], pose[5]);
|
} else if (_cmd[0] == '>' || _cmd[0] == '@')
|
||||||
} else if (argNum == 7)
|
{
|
||||||
{
|
uint32_t freeSize = dummy.commandHandler.Push(_cmd);
|
||||||
dummy.SetJointSpeed(speed);
|
Respond(_responseChannel, "%d", freeSize);
|
||||||
dummy.MoveL(pose[0], pose[1], pose[2],
|
}
|
||||||
pose[3], pose[4], pose[5]);
|
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
|
||||||
}
|
}
|
||||||
|
|
||||||
while (dummy.IsMoving())
|
|
||||||
osDelay(10);
|
void OnUart5AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
|
||||||
Respond(_responseChannel, "ok");
|
{
|
||||||
}
|
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
|
||||||
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
|
|
||||||
}
|
/*---------------------------- ↑ 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"
|
#include "common_inc.h"
|
||||||
|
|
||||||
/*----------------- 1.Add Your Extern Variables Here (Optional) ------------------*/
|
/*----------------- 1.Add Your Extern Variables Here (Optional) ------------------*/
|
||||||
extern DummyRobot dummy;
|
extern DummyRobot dummy;
|
||||||
|
|
||||||
class HelperFunctions
|
class HelperFunctions
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
/*--------------- 2.Add Your Helper Functions Helper Here (optional) ----------------*/
|
/*--------------- 2.Add Your Helper Functions Helper Here (optional) ----------------*/
|
||||||
int32_t TestFunction(int32_t delta)
|
float GetTemperatureHelper()
|
||||||
{
|
{ return AdcGetChipTemperature(); }
|
||||||
static int cnt = 0;
|
|
||||||
return cnt += delta;
|
} staticFunctions;
|
||||||
}
|
|
||||||
|
|
||||||
void SaveConfigurationHelper()
|
// Define options that intractable with "reftool".
|
||||||
{}
|
static inline auto MakeObjTree()
|
||||||
|
{
|
||||||
void EraseConfigurationHelper()
|
/*--------------- 3.Add Your Protocol Variables & Functions Here ----------------*/
|
||||||
{}
|
return make_protocol_member_list(
|
||||||
|
// Add Read-Only Variables
|
||||||
float GetTemperatureHelper()
|
make_protocol_ro_property("serial_number", &serialNumber),
|
||||||
{ return AdcGetChipTemperature(); }
|
make_protocol_function("get_temperature", staticFunctions, &HelperFunctions::GetTemperatureHelper),
|
||||||
|
make_protocol_object("robot", dummy.MakeProtocolDefinitions())
|
||||||
void SystemResetHelper()
|
);
|
||||||
{ NVIC_SystemReset(); }
|
}
|
||||||
|
|
||||||
} staticFunctions;
|
|
||||||
|
COMMIT_PROTOCOL
|
||||||
|
|
||||||
// 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
|
|
||||||
|
@ -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" />
|
<module classpath="CMake" type="CPP_MODULE" version="4" />
|
@ -1,8 +1,8 @@
|
|||||||
<?xml version="1.0" encoding="UTF-8"?>
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
<project version="4">
|
<project version="4">
|
||||||
<component name="ProjectModuleManager">
|
<component name="ProjectModuleManager">
|
||||||
<modules>
|
<modules>
|
||||||
<module fileurl="file://$PROJECT_DIR$/.idea/Ctrl-Step-fw.iml" filepath="$PROJECT_DIR$/.idea/Ctrl-Step-fw.iml" />
|
<module fileurl="file://$PROJECT_DIR$/.idea/Ctrl-Step-fw.iml" filepath="$PROJECT_DIR$/.idea/Ctrl-Step-fw.iml" />
|
||||||
</modules>
|
</modules>
|
||||||
</component>
|
</component>
|
||||||
</project>
|
</project>
|
Loading…
Reference in New Issue
Block a user