static void runChprintfTest() {
static MemoryStream ts;
static char testBuffer[200];
msObjectInit(&ts, (uint8_t *) testBuffer, sizeof(testBuffer), 0);
// it's a very, very long and mostly forgotten story how this became our %.2f precision format
ts.eos = 0; // reset
chprintf((BaseSequentialStream*)&ts, "%.2f/%.4f/%.4f", 0.239f, 239.932, 0.1234);
ts.buffer[ts.eos] = 0;
assertString(testBuffer, "0.23/239.9320/0.1234");
{
LoggingWithStorage testLogging("test");
appendFloat(&testLogging, 1.23, 5);
appendFloat(&testLogging, 1.234, 2);
assertString(testLogging.buffer, "1.230001.23");
}
{
LoggingWithStorage testLogging("test");
appendFloat(&testLogging, -1.23, 5);
assertString(testLogging.buffer, "-1.23000");
}
{
LoggingWithStorage testLogging("test");
appendPrintf(&testLogging, "a%.2fb%fc", -1.2, -3.4);
assertString(testLogging.buffer, "a-1.20b-3.400000095c");
}
}
/*
* Application entry point.
*/
int main(void) {
Thread *shelltp = NULL;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Activates the serial driver 1 using the driver default configuration.
*/
sdStart(&SD1, NULL);
/*
* Shell manager initialization.
*/
shellInit();
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Normal main() thread activity.
*/
while (TRUE) {
if (!shelltp)
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminated(shelltp)) {
chThdRelease(shelltp); /* Recovers memory of the previous shell. */
shelltp = NULL; /* Triggers spawning of a new shell. */
}
#if 0
if (SIU.GPDI[GPIO_BUTTON1].B.PDI) {
volatile msg_t result;
#if 0
MemoryStream report;
msObjectInit(&report, report_buffer, sizeof(report_buffer), 0);
result = TestThread(&report);
#else
result = TestThread(&SD1);
#endif
}
#endif
chThdSleepMilliseconds(1000);
}
return 0;
}
size_t debugPrint(const char *fmt, ...)
/*
printf style debugging output
outputs a trailing newline
*/
{
size_t len;
va_list ap;
va_start(ap, fmt);
static MUTEX_DECL(debugPrintLock);
static uint8_t buf[debugPrintBufSize];
static MemoryStream dbgStream;
chMtxLock(&debugPrintLock);
msObjectInit(&dbgStream, buf, sizeof(buf), 0);
chvprintf((BaseSequentialStream *) &dbgStream, fmt, ap);
va_end(ap);
debugPut(buf, len=dbgStream.eos);
chMtxUnlock();
return len;
}
void initIntermediateLoggingBuffer(void) {
initLoggingCentral();
msObjectInit(&intermediateLoggingBuffer, intermediateLoggingBufferData, INTERMEDIATE_LOGGING_BUFFER_SIZE, 0);
intermediateLoggingBufferInited = TRUE;
}
void runRusEfi(void) {
msObjectInit(&firmwareErrorMessageStream, errorMessageBuffer, sizeof(errorMessageBuffer), 0);
// that's dirty, this assignment should be nicer or in a better spot
engine->engineConfiguration = engineConfiguration;
engine->engineConfiguration2 = engineConfiguration2;
engineConfiguration2->engineConfiguration = engineConfiguration;
initErrorHandling();
/**
* First data structure keeps track of which hardware I/O pins are used by whom
*/
initPinRepository();
/**
* Next we should initialize serial port console, it's important to know what's going on
*/
initializeConsole();
initLogging(&logging, "main");
engine->init();
addConsoleAction("reset", scheduleReset);
/**
* Initialize hardware drivers
*/
initHardware(&logging, engine);
initStatusLoop(engine);
/**
* Now let's initialize actual engine control logic
* todo: should we initialize some? most? controllers before hardware?
*/
initEngineContoller(engine);
#if EFI_PERF_METRICS || defined(__DOXYGEN__)
initTimePerfActions();
#endif
#if EFI_ENGINE_EMULATOR || defined(__DOXYGEN__)
initEngineEmulator(engine);
#endif
startStatusThreads(engine);
print("Running main loop\r\n");
main_loop_started = TRUE;
/**
* This loop is the closes we have to 'main loop' - but here we only publish the status. The main logic of engine
* control is around main_trigger_callback
*/
while (TRUE) {
efiAssertVoid(getRemainingStack(chThdSelf()) > 128, "stack#1");
#if (EFI_CLI_SUPPORT && !EFI_UART_ECHO_TEST_MODE) || defined(__DOXYGEN__)
// sensor state + all pending messages for our own dev console
updateDevConsoleState(engine);
#endif /* EFI_CLI_SUPPORT */
chThdSleepMilliseconds(boardConfiguration->consoleLoopPeriod);
}
}
void initLcdController(void) {
tree.init(&miRpm, engineConfiguration->HD44780height - 1);
msObjectInit(&lcdLineStream, (uint8_t *) lcdLineBuffer, sizeof(lcdLineBuffer), 0);
}
//-----------------------------------------------------------------------------
// This function kinda sucks, but it's understandable.
// We sequentially try to create files called KUROxxxx.KBB, where xxxx 0->0999,
// with the flags set to only open if file doesn't exists. So if the file
// does exists, we get a FR_EXIST return value, and try the next.
static uint8_t
new_file(void)
{
char charbuf[64];
MemoryStream msb;
FRESULT stat = FR_OK;
if ( kbfile.fs )
{
// file is open, close it first
f_close(&kbfile);
}
uint16_t fnum = 0;
for( ; fnum < 1000 ; fnum++ )
{
// the joys of doing an sprintf();
memset(charbuf,0,sizeof(charbuf));
msObjectInit(&msb, (uint8_t*)charbuf, sizeof(charbuf), 0);
chprintf((BaseSequentialStream *)&msb, "%s/%s%.4d%s", NANIBOX_DNAME,
KUROBOX_FNAME_STEM, fnum, KUROBOX_FNAME_EXT);
stat = f_open(&kbfile, charbuf, FA_WRITE|FA_CREATE_NEW);
if (stat == FR_EXIST)
// it exists, just try the next
continue;
else if (stat != FR_OK)
// we got a return code that's not FR_EXIST or FR_OK, which means
// a different type of error, return from this.
return stat;
else
// new file created, lets break from loop
break;
}
// we can only get an OK if we opened the file
if (stat == FR_OK)
{
// make sure that changes are flushed out to disk
f_sync(&kbfile);
memset(charbuf,0,sizeof(charbuf));
msObjectInit(&msb, (uint8_t*)charbuf, sizeof(charbuf), 0);
chprintf((BaseSequentialStream *)&msb, "%s%.4d", KUROBOX_FNAME_STEM, fnum);
// since we don't want the full path or the extension, lets do that here
kbs_setFName(charbuf);
// here we write out the header to the file
uint8_t * buf = (uint8_t*) &header_msg;
// but first, checksum...
header_msg.header.checksum = calc_checksum_16(buf+KBB_CHECKSUM_START,
KBB_MSG_SIZE-KBB_CHECKSUM_START);
UINT bytes_written = 0;
stat = f_write(&kbfile, buf, header_msg.header.msg_size, &bytes_written);
if (bytes_written != sizeof(header_msg.header.msg_size) || stat != FR_OK)
current_msg.write_errors++;
stat = f_sync(&kbfile);
if (stat != FR_OK)
current_msg.write_errors++;
}
return stat;
}
static THD_FUNCTION(sdlog, arg)
{
(void)arg;
chRegSetThreadName("sdlog");
static event_listener_t sensor_listener;
chEvtRegisterMaskWithFlags(&sensor_events, &sensor_listener,
(eventmask_t)ONBOARDSENSOR_EVENT,
(eventflags_t)SENSOR_EVENT_MPU6000);
static uint8_t writebuf[200];
static MemoryStream writebuf_stream;
bool error = false;
UINT _bytes_written;
FRESULT res;
static FIL version_fd;
res = f_open(&version_fd, "/log/version", FA_WRITE | FA_CREATE_ALWAYS);
if (res) {
chprintf(stdout, "error %d opening %s\n", res, "/log/version");
return;
}
msObjectInit(&writebuf_stream, writebuf, sizeof(writebuf), 0);
chprintf((BaseSequentialStream*)&writebuf_stream,
"git version: %s (%s)\n"
"compiler: %s\n"
"built: %s\n",
build_git_version, build_git_branch,
PORT_COMPILER_NAME,
build_date);
error = error || f_write(&version_fd, writebuf, writebuf_stream.eos, &_bytes_written);
f_close(&version_fd);
static FIL mpu6000_fd;
res = f_open(&mpu6000_fd, "/log/mpu6000.csv", FA_WRITE | FA_CREATE_ALWAYS);
if (res) {
chprintf(stdout, "error %d opening %s\n", res, "/log/mpu6000.csv");
return;
}
const char *mpu_descr = "time,gyro_x,gyro_y,gyro_z,acc_x,acc_y,acc_z,temp\n";
error |= error || f_write(&mpu6000_fd, mpu_descr, strlen(mpu_descr), &_bytes_written);
static FIL rc_fd;
res = f_open(&rc_fd, "/log/rc.csv", FA_WRITE | FA_CREATE_ALWAYS);
if (res) {
chprintf(stdout, "error %d opening %s\n", res, "/log/rc.csv");
return;
}
const char *rc_descr = "time,signal,ch1,ch2,ch3,ch4,ch5\n";
error |= error || f_write(&rc_fd, rc_descr, strlen(rc_descr), &_bytes_written);
while (!error) {
eventmask_t events = chEvtWaitAny(ONBOARDSENSOR_EVENT);
float t = (float)chVTGetSystemTimeX() / CH_CFG_ST_FREQUENCY;
if (events & ONBOARDSENSOR_EVENT) {
chEvtGetAndClearFlags(&sensor_listener);
rate_gyro_sample_t gyro;
accelerometer_sample_t acc;
onboard_sensor_get_mpu6000_gyro_sample(&gyro);
onboard_sensor_get_mpu6000_acc_sample(&acc);
float temp = onboard_sensor_get_mpu6000_temp();
msObjectInit(&writebuf_stream, writebuf, sizeof(writebuf), 0);
chprintf((BaseSequentialStream*)&writebuf_stream,
"%f,%f,%f,%f,%f,%f,%f,%f\n", gyro.timestamp, gyro.rate[0], gyro.rate[1], gyro.rate[2], acc.acceleration[0], acc.acceleration[1], acc.acceleration[2], temp);
UINT _bytes_written;
int ret = f_write(&mpu6000_fd, writebuf, writebuf_stream.eos, &_bytes_written);
if (ret != 0) {
chprintf(stdout, "write failed %d\n", ret);
}
if (ret == 9) {
f_open(&mpu6000_fd, "/log/mpu6000.csv", FA_WRITE);
f_lseek(&mpu6000_fd, f_size(&mpu6000_fd));
}
static int sync_needed = 0;
sync_needed++;
if (sync_needed == 100) {
f_sync(&mpu6000_fd);
sync_needed = 0;
}
}
if (false) {
static struct rc_input_s rc_in;
sumd_input_get(&rc_in);
msObjectInit(&writebuf_stream, writebuf, sizeof(writebuf), 0);
chprintf((BaseSequentialStream*)&writebuf_stream,
"%f,%d,%f,%f,%f,%f,%f\n", t, !rc_in.no_signal, rc_in.channel[0], rc_in.channel[1], rc_in.channel[2], rc_in.channel[3], rc_in.channel[4]);
UINT _bytes_written;
int ret = f_write(&rc_fd, writebuf, writebuf_stream.eos, &_bytes_written);
if (ret != 0) {
chprintf(stdout, "write failed %d\n", ret);
}
static int sync_needed = 0;
sync_needed++;
if (sync_needed == 100) {
f_sync(&rc_fd);
sync_needed = 0;
}
}
}
}
