-
Notifications
You must be signed in to change notification settings - Fork 1
/
CubeDefines.cpp
126 lines (107 loc) · 4.92 KB
/
CubeDefines.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
/*
* CubeDefines.cpp
*
* Created on: Oct 18, 2023
* Author: Chris
*/
#include <cstdarg> // Support for va_start and va_end
#include <cstring> // Support for strlen and strcpy
#include "Core/Inc/Mutex.hpp"
#include "Core/Inc/Command.hpp"
#include "Drivers/Inc/UARTDriver.hpp"
#include "CubeDefines.hpp"
#include "SystemDefines.hpp"
#include "CubeTask.hpp"
/* Global Variables ------------------------------------------------------------------*/
Mutex Global::vaListMutex;
/* System Functions ------------------------------------------------------------*/
/**
* @brief Variadic print function, sends a command packet to the queue
* @param str String to print with printf style formatting
* @param ... Additional arguments to print if assertion fails, in same format as printf
*/
void cube_print(const char* str, ...)
{
#ifndef DISABLE_DEBUG
//Try to take the VA list mutex
if (Global::vaListMutex.Lock(DEBUG_TAKE_MAX_TIME_MS)) {
// If we have a message, and can use VA list, extract the string into a new buffer, and null terminate it
uint8_t str_buffer[DEBUG_PRINT_MAX_SIZE] = {};
va_list argument_list;
va_start(argument_list, str);
int16_t buflen = vsnprintf(reinterpret_cast<char*>(str_buffer), sizeof(str_buffer) - 1, str, argument_list);
va_end(argument_list);
if (buflen > 0) {
str_buffer[buflen] = '\0';
}
// Release the VA List Mutex
Global::vaListMutex.Unlock();
//Generate a command
Command cmd(DATA_COMMAND, (uint16_t)CUBE_TASK_COMMAND_SEND_DEBUG); // Set the UART channel to send data on
//Copy data into the command
cmd.CopyDataToCommand(str_buffer, buflen);
//Send this packet off to the UART Task
CubeTask::Inst().GetEventQueue()->Send(cmd, false);
}
else
{
// Print out that we could not acquire the VA list mutex
CUBE_ASSERT(false, "Could not acquire VA_LIST mutex");
}
#endif
}
/**
* @brief Variadic assertion function, wraps assert for multi-platform support and debug builds
* @param condition Assertion that this condition is true (!0)
* @param file File that the assertion is in (__FILE__)
* @param line Line number that the assertion is on (__LINE__)
* @param str Optional message to print if assertion fails. Must be less than 192 characters AFTER formatting
* @param ... Additional arguments to print if assertion fails, in same format as printf
*/
void cube_assert_debug(bool condition, const char* file, const uint16_t line, const char* str, ...) {
// If assertion succeeds, do nothing
if (condition) {
return;
}
#ifndef DISABLE_DEBUG
bool printMessage = false;
// NOTE: Be careful! If va_list funcs while RTOS is active ALL calls to any vsnprint functions MUST have a mutex lock/unlock
// NOTE: https://nadler.com/embedded/newlibAndFreeRTOS.html
// We have an assert fail, we try to take control of the Debug semaphore, and then suspend all other parts of the system
if (Global::vaListMutex.Lock(ASSERT_TAKE_MAX_TIME_MS)) {
// We have the mutex, we can now safely print the message
printMessage = true;
}
vTaskSuspendAll();
//If we have the vaListMutex, we can safely use vsnprintf
if (printMessage) {
// Print out the assertion header through the supported interface, we don't have a UART task running, so we directly use HAL
uint8_t header_buf[ASSERT_BUFFER_MAX_SIZE] = {};
int16_t res = snprintf(reinterpret_cast<char*>(header_buf), ASSERT_BUFFER_MAX_SIZE - 1, "\r\n\n-- ASSERTION FAILED --\r\nFile [%s] @ Line # [%d]\r\n", file, line);
if (res < 0) {
// If we failed to generate the header, just format the line number
snprintf(reinterpret_cast<char*>(header_buf), ASSERT_BUFFER_MAX_SIZE - 1, "\r\n\n-- ASSERTION FAILED --\r\nFile [PATH_TOO_LONG] @ Line # [%d]\r\n", line);
}
// Output the header to the debug port
DEFAULT_DEBUG_UART_DRIVER->Transmit(header_buf, strlen(reinterpret_cast<char*>(header_buf)));
// If we have a message, and can use VA list, extract the string into a new buffer, and null terminate it
if (printMessage && str != nullptr) {
uint8_t str_buffer[ASSERT_BUFFER_MAX_SIZE] = {};
va_list argument_list;
va_start(argument_list, str);
int16_t buflen = vsnprintf(reinterpret_cast<char*>(str_buffer), sizeof(str_buffer) - 1, str, argument_list);
va_end(argument_list);
if (buflen > 0) {
str_buffer[buflen] = '\0';
DEFAULT_DEBUG_UART_DRIVER->Transmit(str_buffer, buflen);
}
}
}
else {
DEFAULT_DEBUG_UART_DRIVER->Transmit((uint8_t*)"-- ASSERTION FAILED --\r\nCould not acquire vaListMutex\r\n", 55);
}
#endif
HAL_NVIC_SystemReset();
// We should not reach this code, but if we do, we should resume the scheduler
xTaskResumeAll();
}