diff --git a/software/dumber3/.cproject b/software/dumber3/.cproject index 8e203ea..fe1e973 100644 --- a/software/dumber3/.cproject +++ b/software/dumber3/.cproject @@ -327,6 +327,18 @@ + + + + + + + + + + - - - - - - - - - diff --git a/software/dumber3/Application/application.c b/software/dumber3/Application/application.c index 59a5fc8..cb166cf 100644 --- a/software/dumber3/Application/application.c +++ b/software/dumber3/Application/application.c @@ -125,8 +125,10 @@ void APPLICATION_MainThread(void* params) { if (receivedCMD != NULL) { decodedCmd = cmdDecode(receivedCMD,strlen(receivedCMD)); - if (decodedCmd==CMD_DECODE_UNKNOWN) + if (decodedCmd->type==CMD_NONE) cmdSendAnswer(ANS_UNKNOWN); + else if (decodedCmd->type == CMD_INVALID_CHECKSUM) + cmdSendAnswer(ANS_ERR); else { systemInfos.cmd = decodedCmd->type; systemTimeout.inactivityCnt = 0; @@ -164,8 +166,9 @@ void APPLICATION_MainThread(void* params) { */ break; } - free(decodedCmd); } + + free(decodedCmd); break; default: diff --git a/software/dumber3/Application/commands.c b/software/dumber3/Application/commands.c index 152e4dd..135c81b 100644 --- a/software/dumber3/Application/commands.c +++ b/software/dumber3/Application/commands.c @@ -8,6 +8,7 @@ #include "commands.h" #include #include +#include /* Definition des commandes */ @@ -75,7 +76,7 @@ char* cmdAddChecksum(const char* str) { * Si celui-ci est bon, ll retournera 0 et supprimera le checksum de str * sinon il retournera 1 sans faire de modification. * @param None - * @retval 0 ou 1 + * @retval 0 si le checksum est faux, 1 sinon * */ char cmdVerifyChecksum(char* str) { @@ -84,131 +85,130 @@ char cmdVerifyChecksum(char* str) { unsigned char checksum=0; length = strlen(str); - for (j = 0; j < length - 2; j++) { + /* Warning: str should be without ending CR (0x0D) character, so, a ping command should be + * received as "pp", 2 chars long + * + * in the loop after, ending length is length of the caommand string whithout last char, the checksum + * so, we have jtype = CMD_MOVE; -// ((CMD_Move*)decodedCmd)->distance = ((int16_t)cmd[1]<<8) + (int16_t)cmd[2]; -// break; -// -// case CMD_TURN: -// decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Turn)); -// decodedCmd->type = CMD_TURN; -// ((CMD_Turn*)decodedCmd)->turns = ((int16_t)cmd[1]<<8) + (int16_t)cmd[2]; -// break; -// -// case CMD_PING: -// case CMD_RESET: -// case CMD_START_WITHOUT_WATCHDOG: -// case CMD_START_WITH_WATCHDOG: -// case CMD_RESET_WATCHDOG: -// case CMD_GET_BATTERY: -// case CMD_GET_BUSY_STATE: -// case CMD_GET_VERSION: -// case CMD_TEST: -// case CMD_DEBUG: -// case CMD_POWER_OFF: -// decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); -// decodedCmd->type = cmd_type; -// break; -// -// default: -// decodedCmd = CMD_DECODE_UNKNOWN; -// } -// -// return decodedCmd; -//} - CMD_Generic* cmdDecode(char* cmd, uint8_t length) { CMD_Generic* decodedCmd; char cmd_type = cmd[0]; + char *p; - switch (cmd_type) - { - case MoveCMD: - decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Move)); - decodedCmd->type = CMD_MOVE; - ((CMD_Move*)decodedCmd)->distance = ((int16_t)cmd[1]<<8) + (int16_t)cmd[2]; - break; + /* First, verify checksum */ + if (cmdVerifyChecksum(cmd)) { + switch (cmd_type) + { + case MoveCMD: + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Move)); + decodedCmd->type = CMD_MOVE; + //((CMD_Move*)decodedCmd)->distance = ((int16_t)cmd[1]<<8) + (int16_t)cmd[2]; - case TurnCMD: - decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Turn)); - decodedCmd->type = CMD_TURN; - ((CMD_Turn*)decodedCmd)->turns = ((int16_t)cmd[1]<<8) + (int16_t)cmd[2]; - break; + /* verify that command start with "M=" */ + if ((cmd[0]=='M')&&(cmd[1]=='=')) { + cmd = cmd+2; //cmd+2 for removing "M=" at start of the string + ((CMD_Move*)decodedCmd)->distance=strtoul(cmd , &p, 10); + if (p==cmd) + decodedCmd->type = CMD_NONE; /* missing number value xxxxx in "M=xxxxx" */ + } else + decodedCmd->type = CMD_NONE; /* misformed command (should start with "M=" */ + break; - case PingCMD: - decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); - decodedCmd->type = CMD_PING; - break; - case ResetCMD: - decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); - decodedCmd->type = CMD_RESET; - break; - case StartWWatchDogCMD: - decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); - decodedCmd->type = CMD_START_WITH_WATCHDOG; - break; - case StartWithoutWatchCMD: - decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); - decodedCmd->type = CMD_START_WITHOUT_WATCHDOG; - break; - case ResetWatchdogCMD: - decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); - decodedCmd->type = CMD_RESET_WATCHDOG; - break; - case GetBatteryVoltageCMD: - decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); - decodedCmd->type = CMD_GET_BATTERY; - break; - case GetVersionCMD: - decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); - decodedCmd->type = CMD_GET_VERSION; - break; - case BusyStateCMD: - decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); - decodedCmd->type = CMD_GET_BUSY_STATE; - break; - case TestCMD: - decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); - decodedCmd->type = CMD_TEST; - break; - case DebugCMD: - decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); - decodedCmd->type = CMD_DEBUG; - break; - case PowerOffCMD: - decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); - decodedCmd->type = CMD_POWER_OFF; - break; + case TurnCMD: + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Turn)); + decodedCmd->type = CMD_TURN; + //((CMD_Turn*)decodedCmd)->turns = ((int16_t)cmd[1]<<8) + (int16_t)cmd[2]; - default: - decodedCmd = CMD_DECODE_UNKNOWN; + //if (!sscanf(cmd,"T=%hd",&((CMD_Turn*)decodedCmd)->turns )) + // decodedCmd->type = CMD_NONE; + + /* verify that command start with "T=" */ + if ((cmd[0]=='T')&&(cmd[1]=='=')) { + cmd = cmd+2; //cmd+2 for removing "T=" at start of the string + ((CMD_Turn*)decodedCmd)->turns=strtoul(cmd , &p, 10); + if (p==cmd) + decodedCmd->type = CMD_NONE; /* missing number value xxxxx in "T=xxxxx" */ + } else + decodedCmd->type = CMD_NONE; /* misformed command (should start with "T=" */ + break; + + case PingCMD: + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); + decodedCmd->type = CMD_PING; + break; + case ResetCMD: + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); + decodedCmd->type = CMD_RESET; + break; + case StartWWatchDogCMD: + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); + decodedCmd->type = CMD_START_WITH_WATCHDOG; + break; + case StartWithoutWatchCMD: + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); + decodedCmd->type = CMD_START_WITHOUT_WATCHDOG; + break; + case ResetWatchdogCMD: + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); + decodedCmd->type = CMD_RESET_WATCHDOG; + break; + case GetBatteryVoltageCMD: + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); + decodedCmd->type = CMD_GET_BATTERY; + break; + case GetVersionCMD: + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); + decodedCmd->type = CMD_GET_VERSION; + break; + case BusyStateCMD: + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); + decodedCmd->type = CMD_GET_BUSY_STATE; + break; + case TestCMD: + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); + decodedCmd->type = CMD_TEST; + break; + case DebugCMD: + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); + decodedCmd->type = CMD_DEBUG; + break; + case PowerOffCMD: + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); + decodedCmd->type = CMD_POWER_OFF; + break; + + default: + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); + decodedCmd->type = CMD_NONE; + } + } else { /* Checksum is wrong*/ + decodedCmd = (CMD_Generic*)malloc(sizeof(CMD_Generic)); + decodedCmd->type = CMD_INVALID_CHECKSUM; } return decodedCmd; diff --git a/software/dumber3/Application/commands.h b/software/dumber3/Application/commands.h index fe091ee..3a1caac 100644 --- a/software/dumber3/Application/commands.h +++ b/software/dumber3/Application/commands.h @@ -24,7 +24,8 @@ typedef enum { CMD_GET_BUSY_STATE, CMD_TEST, CMD_DEBUG, - CMD_POWER_OFF + CMD_POWER_OFF, + CMD_INVALID_CHECKSUM=0xFF } CMD_CommandsType; typedef enum { @@ -77,9 +78,6 @@ typedef struct __attribute__((packed)) { uint8_t state; } ANS_Busy_State; -#define CMD_DECODE_INVALID ((CMD_Generic*)NULL) -#define CMD_DECODE_UNKNOWN ((CMD_Generic*)UINT32_MAX) - CMD_Generic* cmdDecode(char* cmd, uint8_t length); void cmdSendAnswer(uint8_t ans); void cmdSendString(char* str); diff --git a/software/dumber3/Application/moteurs.c b/software/dumber3/Application/moteurs.c index 9291e22..0399c32 100644 --- a/software/dumber3/Application/moteurs.c +++ b/software/dumber3/Application/moteurs.c @@ -105,15 +105,21 @@ void MOTEURS_Init(void) { void MOTEURS_Avance(uint32_t distance) { static uint32_t dist; - dist = distance; - MESSAGE_SendMailbox(MOTEURS_Mailbox, MSG_ID_MOTEURS_MOVE, APPLICATION_Mailbox, (void*)dist); + if (distance) { + dist = distance; + MESSAGE_SendMailbox(MOTEURS_Mailbox, MSG_ID_MOTEURS_MOVE, APPLICATION_Mailbox, (void*)&dist); + } else + MOTEURS_Stop(); } void MOTEURS_Tourne(uint32_t tours) { static uint32_t turns; - turns = tours; - MESSAGE_SendMailbox(MOTEURS_Mailbox, MSG_ID_MOTEURS_TURN, APPLICATION_Mailbox, (void*)turns); + if (tours) { + turns = tours; + MESSAGE_SendMailbox(MOTEURS_Mailbox, MSG_ID_MOTEURS_TURN, APPLICATION_Mailbox, (void*)&turns); + } else + MOTEURS_Stop(); } void MOTEURS_Stop(void) { diff --git a/software/dumber3/Application/xbee.c b/software/dumber3/Application/xbee.c index d8e2551..c35e1d8 100644 --- a/software/dumber3/Application/xbee.c +++ b/software/dumber3/Application/xbee.c @@ -131,28 +131,28 @@ int XBEE_SendData(char* data) { int status = XBEE_OK; // Prevents successive calls to overlap - //state = xSemaphoreTake(xHandleSemaphoreTX, pdMS_TO_TICKS(XBEE_TX_SEMAPHORE_WAIT)); // wait max 500 ms (to avoid interlocking) + state = xSemaphoreTake(xHandleSemaphoreTX, pdMS_TO_TICKS(XBEE_TX_SEMAPHORE_WAIT)); // wait max 500 ms (to avoid interlocking) - //if (state != pdFALSE) { /* test semaphore take answer - // if answer is false, it means timeout appends - // We should probably reset something in "else" branch */ + if (state != pdFALSE) { /* test semaphore take answer + if answer is false, it means timeout appends + We should probably reset something in "else" branch */ - while (LL_USART_IsEnabledIT_TXE(hlpuart1.Instance)) { - vTaskDelay(pdMS_TO_TICKS(1)); - } + // while (LL_USART_IsEnabledIT_TXE(hlpuart1.Instance)) { + // vTaskDelay(pdMS_TO_TICKS(1)); + // } - strncpy((char*)txBuffer,data,XBEE_TX_BUFFER_MAX_LENGTH-1); - txBuffer[XBEE_TX_BUFFER_MAX_LENGTH-1]=0; - txRemainingData = strlen((char*)txBuffer); + strncpy((char*)txBuffer,data,XBEE_TX_BUFFER_MAX_LENGTH-1); + txBuffer[XBEE_TX_BUFFER_MAX_LENGTH-1]=0; + txRemainingData = strlen((char*)txBuffer); - if (txRemainingData!=0) { - txIndex =1; - txRemainingData=txRemainingData-1; + if (txRemainingData!=0) { + txIndex =1; + txRemainingData=txRemainingData-1; - LL_USART_TransmitData8(hlpuart1.Instance, txBuffer[0]); - LL_USART_EnableIT_TXE(hlpuart1.Instance); // enable TX Interrupt - } - //} else status= XBEE_TX_TIMEOUT; + LL_USART_TransmitData8(hlpuart1.Instance, txBuffer[0]); + LL_USART_EnableIT_TXE(hlpuart1.Instance); // enable TX Interrupt + } + } else status= XBEE_TX_TIMEOUT; return status; } @@ -162,7 +162,7 @@ void XBEE_TX_IRQHandler(void) { if (txRemainingData==0) { // No more data, disable TXE bit LL_USART_DisableIT_TXE(hlpuart1.Instance); - //xSemaphoreGiveFromISR( xHandleSemaphoreTX, &xHigherPriorityTaskWoken ); + xSemaphoreGiveFromISR( xHandleSemaphoreTX, &xHigherPriorityTaskWoken ); } else { LL_USART_TransmitData8(hlpuart1.Instance, txBuffer[txIndex]); txIndex++; diff --git a/software/dumber3/Core/Src/stm32l0xx_hal_msp.c b/software/dumber3/Core/Src/stm32l0xx_hal_msp.c index 0f64c61..b72334d 100644 --- a/software/dumber3/Core/Src/stm32l0xx_hal_msp.c +++ b/software/dumber3/Core/Src/stm32l0xx_hal_msp.c @@ -181,7 +181,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* huart) HAL_NVIC_SetPriority(LPUART1_IRQn, 3, 0); HAL_NVIC_EnableIRQ(LPUART1_IRQn); /* USER CODE BEGIN LPUART1_MspInit 1 */ - //HAL_NVIC_SetPriority(LPUART1_IRQn, 3, 0); + //HAL_NVIC_SetPriority(LPUART1_IRQn, 2, 0); /* USER CODE END LPUART1_MspInit 1 */ } diff --git a/software/dumber3/Core/Src/syscalls.c b/software/dumber3/Core/Src/syscalls.c index 73a34f9..0476b77 100644 --- a/software/dumber3/Core/Src/syscalls.c +++ b/software/dumber3/Core/Src/syscalls.c @@ -39,10 +39,11 @@ extern int __io_getchar(void) __attribute__((weak)); extern uint8_t ucHeap[ configTOTAL_HEAP_SIZE ]; - char *__env[1] = { 0 }; char **environ = __env; +uint32_t Counter_Malloc=0; +uint32_t Counter_Free=0; /* Functions */ void* malloc(size_t size) @@ -53,6 +54,7 @@ void* malloc(size_t size) { // We simply wrap the FreeRTOS call into a standard form ptr = pvPortMalloc(size); + Counter_Malloc++; } // else NULL if there was an error return ptr; @@ -65,6 +67,7 @@ void free(void* ptr) if ((ptr>=(void*)ucHeap) && (ptr<=(void*)ucHeap+configTOTAL_HEAP_SIZE)) { // We simply wrap the FreeRTOS call into a standard form vPortFree(ptr); + Counter_Free++; } } } diff --git a/software/dumber3/Dumber3 Debug.launch b/software/dumber3/Dumber3 Debug.launch deleted file mode 100644 index 98197f0..0000000 --- a/software/dumber3/Dumber3 Debug.launch +++ /dev/null @@ -1,79 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/software/dumber3/TestsPlans/tests.c b/software/dumber3/TestsPlans/tests.c index b7d9a4b..c22cb38 100644 --- a/software/dumber3/TestsPlans/tests.c +++ b/software/dumber3/TestsPlans/tests.c @@ -35,7 +35,7 @@ typedef enum { MISC_Tests } TESTS_Type; -TESTS_Type TESTS_Nbr=COMMANDS_Tests; // Number indicating which test is being run +TESTS_Type TESTS_Nbr=MOTEURS_Tests; // Number indicating which test is being run void TESTS_BasicTests(void* params); @@ -171,12 +171,12 @@ void TESTS_BasicTests(void* params) { break; case CMD_MOVE: - snprintf(str, 99, "MOVE received\r"); + snprintf(str, 99, "MOVE received (dist=%hd)\r",((CMD_Move*)cmd)->distance); cmdSendAnswer(ANS_OK); break; case CMD_TURN: - snprintf(str, 99, "TURN received\r"); + snprintf(str, 99, "TURN received (turns=%hd)\r",((CMD_Turn*)cmd)->turns); cmdSendAnswer(ANS_OK); break; case CMD_GET_BUSY_STATE: @@ -199,14 +199,22 @@ void TESTS_BasicTests(void* params) { snprintf(str, 99, "POWER_OFF received\r"); cmdSendAnswer(ANS_OK); break; - default: + case CMD_NONE: + snprintf(str, 99, "Unknown command\r"); + cmdSendAnswer(ANS_UNKNOWN); + break; + case CMD_INVALID_CHECKSUM: + snprintf(str, 99, "Invalid checksum\r"); + cmdSendAnswer(ANS_ERR); + break; + default: snprintf(str, 99, "Unknown command\r"); cmdSendAnswer(ANS_UNKNOWN); break; } - + free(cmd); MESSAGE_SendMailbox(XBEE_Mailbox, MSG_ID_XBEE_ANS, APPLICATION_Mailbox, (void*)str); } } @@ -246,6 +254,40 @@ void TESTS_BasicTests(void* params) { break; case MOTEURS_Tests: + while (1) { + msg = MESSAGE_ReadMailbox(APPLICATION_Mailbox); // Wait for a message from Xbee + + if (msg.id == MSG_ID_XBEE_CMD) { + cmd = cmdDecode((char*)msg.data, strlen((char*)msg.data)); + free(msg.data); + + str[0]=0; + switch (cmd->type) { + case CMD_RESET: + snprintf(str, 99, "RESET received (stop motors)\r"); + cmdSendAnswer(ANS_OK); + MOTEURS_Stop(); + break; + case CMD_MOVE: + snprintf(str, 99, "MOVE received (dist=%hd)\r",((CMD_Move*)cmd)->distance); + cmdSendAnswer(ANS_OK); + MOTEURS_Avance(((CMD_Move*)cmd)->distance); + break; + case CMD_TURN: + snprintf(str, 99, "TURN received (turns=%hd)\r",((CMD_Turn*)cmd)->turns); + cmdSendAnswer(ANS_OK); + MOTEURS_Tourne(((CMD_Turn*)cmd)->turns); + break; + default: + snprintf(str, 99, "Motor test: cmd M or T et R only\r"); + cmdSendAnswer(ANS_ERR); + break; + } + + free(cmd); + MESSAGE_SendMailbox(XBEE_Mailbox, MSG_ID_XBEE_ANS, APPLICATION_Mailbox, (void*)str); + } + } break; case MISC_Tests: // test du bouton on/off diff --git a/software/dumber3/commands.xml b/software/dumber3/commands.xml index e6466bf..c182580 100644 --- a/software/dumber3/commands.xml +++ b/software/dumber3/commands.xml @@ -1,7 +1,7 @@ - false + true 1 500 @@ -11,5 +11,56 @@ 72720D + + 57570D + + + 77770D + + + 75750D + + + 4D3D3230720D + + + 4D3D2D32305F0D + + + 543D3930600D + + + 543D2D39304D0D + + + 76760D + + + 56560D + + + 62620D + + + 74740D + + + 61610D + + + 7A7A0D + + + 4A4A0D + + + 543D2D39303D0D + + + 543D2D3332373637730D + + + 4D3D2D33323736376A0D +