scottp
295abb37ee
Firmware for an autonomous wave-measurement buoy (ATmega2560-based WII5 v2 board). Reads wave motion from a Sparton AHRS-M1/M2 IMU, samples GPS and battery state, and reports back over Iridium SBD satellite telemetry. Originally developed 2012-2024. This is the first public release. Code, documentation, and field-tested operating modes (Capture, Sleep, Position, ManualTest, SelfTest, LowBattery) are licensed under Apache 2.0 — see LICENSE and NOTICE. See README.md for an overview and build instructions, CONTRIBUTING.md for how to contribute, and DEPLOYMENTS.md for the field-deployment log.
1077 lines
33 KiB
C++
1077 lines
33 KiB
C++
// SPDX-License-Identifier: Apache-2.0
|
|
// Copyright (c) 2012-2024 Scott Penrose <scottp@dd.com.au> and WII5 Buoy contributors
|
|
//
|
|
// This file is part of WII5 Buoy firmware.
|
|
// See LICENSE for full terms.
|
|
|
|
/**
|
|
* @file WII5Sparton.cpp
|
|
* @brief Sparton AHRS-M1/M2 IMU driver: capture timing, NorthTek configuration.
|
|
*/
|
|
|
|
/*
|
|
|
|
WII5Sparton
|
|
|
|
TODO 2024 - Review code, check TODO and add some Command functions for testing
|
|
TODO 2024 - Look at getting debug working with serial passthrough in text stage not binary
|
|
TODO 2024 - When getting no data - stop capture and report failure for Iridium
|
|
TODO 2024 - How to send an error - look at Command return string
|
|
TODO 2024 - Debugging text modes?
|
|
|
|
BINARY File
|
|
https://forum.pjrc.com/threads/55114-SD-Datalogging-Best-Practice-in-2019
|
|
Datalogging is a problem I've been trying to brute-force for half a year now.
|
|
|
|
My solution (that I hope to release in a couple more months of testing) runs on top of the SdFs library for the purpose of low-latency high-frequency binary logging. I'm currently able to (somewhat reliably) maintain a logging speed of 925bytes at 500Hz (452KB/s). So in short, it is possible to have a good logging system, although it's highly dependent on how you interact with your data.
|
|
|
|
My current best practises:
|
|
|
|
1. Initialize the SdFs on SdioConfig(FIFO_SDIO) mode.
|
|
2. Preallocate the maximum length of your log file using the .preAllocate(size_t size) method.
|
|
3. Important: Pre-erase the entire file with zeroes.
|
|
4. Before you start logging (to ensure the file is committed fully to the card), call sync().
|
|
5. Write in chunks of a multiple of 512bytes only. (512, 1024, 2048 byte chucks per write() command)
|
|
6. To minimize blocking, try to write only if .card()->isBusy() returns false. This seems to be related to the internal card data management (I'm guessing things like TRIM and what not). Step 3 helps ensure that the card spends as little time as possible in this busy time.
|
|
7. Use a SLC-type SD card. I cannot stress enough the difference this makes. Before buying this card, I had write() operations take up to 10ms on a SanDisk card, but with this expensive card they 99.9% of the time stayed below 500us.
|
|
8. Call truncate() and then sync() after you've finished writing your entire file. The first function removes any extra pre-allocation and the second makes sure that it's a proper FAT32 file. Note that if you're using an exFAT partition your file will not be saved until you call sync() or close().
|
|
7. On that note, FAT32 is always faster than exFAT, but you do have less capacity.
|
|
|
|
I'm still learning more about best practises for logging. My current hang-ups are (I think) related to heap-overflow because of the many sensors I have attached interrupting one another during a write() operation. I'm a very fresh programmer so it's taking me a while but I hope the tips above are helpful enough for you to get started on your 100Hz project.
|
|
|
|
Good luck! Let me know if you find any new practises that I can also test.
|
|
|
|
|
|
64Hz Performance Issue:
|
|
|
|
1 - Version of code that uses standard library loge to disk. No parsing at all.
|
|
This will not allow us to show the 1 Hz update.
|
|
|
|
Status: On Hold
|
|
|
|
2 - Version of the code that uses new library to buffer and then parse int the loops
|
|
|
|
Status: WIP
|
|
|
|
3 - Binary Version: Most likely in interrupt
|
|
|
|
TODO - Cycle through all the baud rates until we get an OK/Huh so we can
|
|
work out it is wokring. Send the stop command as the test. Clear wii5BufferString.
|
|
Send twice, that sort of thing. If not our correct baud rate, then
|
|
send the program command change to new baud.
|
|
|
|
PINS:
|
|
|
|
* 1 - Ground
|
|
* 2 - TX (actually TX from AVR, so Receive)
|
|
* 3 - RX
|
|
* 4 - Power (+4 volts)
|
|
* 5 - NC
|
|
* 6 - RESET (low is reset, keep high, has weak pullup)
|
|
|
|
Special Notes:
|
|
|
|
* 5 millisecond line delay
|
|
*
|
|
|
|
baud 4 set drop<cr>
|
|
|
|
4 = 9600
|
|
5 = 19200
|
|
6 = 38400
|
|
7 = 57600
|
|
8 = 115200
|
|
|
|
Text CRC
|
|
|
|
TODO - We should at least check we have not lost characters
|
|
|
|
Binary.
|
|
|
|
Try RFS (Remote Function Select) Sparton Binary format with CRC etc
|
|
|
|
STATS:Sparton:loops=128008 ERROR:time=512 accel=96
|
|
Standard wii5BufferStrings
|
|
64Hz
|
|
|
|
# INFO: 2024-03-13T23:49:09 - SPARTON: Records received = 1001
|
|
STATS:Sparton:Status=unknown:age=165s
|
|
STATS:Sparton:Time:Total=0
|
|
STATS:Sparton:SerialManager:TODO
|
|
STATS:Sparton:loops=587117 ERROR:time=119 accel=3
|
|
8Hz, SD Card etc
|
|
|
|
# INFO: 2024-03-13T23:50:32 - SPARTON: Records received = 1001
|
|
STATS:Sparton:Status=unknown:age=248s
|
|
STATS:Sparton:Time:Total=0
|
|
STATS:Sparton:SerialManager:TODO
|
|
STATS:Sparton:loops=44851 ERROR:time=199 accel=32
|
|
64Hz, SD Card etc
|
|
|
|
*/
|
|
|
|
#include <Arduino.h>
|
|
#include <WII5Sparton.h>
|
|
|
|
// Careful - loss of serial data with this on
|
|
// #ifdef SPARTON_SERIAL_DEBUG
|
|
|
|
// #define WII5_ZONLY
|
|
// #define WII5_STRINGUTIL
|
|
// #ifdef WII5_STRINGUTIL
|
|
// #include <StringUtil.h>
|
|
// using namespace StringUtil;
|
|
// #endif
|
|
|
|
// INTERRUPT - Keep fast and use volatile varialbes.
|
|
void WII5Sparton::handleRxChar( uint8_t c ) {
|
|
// Which record we playing with?
|
|
currentRecord = (recordCount % SPARTON_RECORDS);
|
|
|
|
// \r and \n for records is an issue...
|
|
|
|
// START of Binary
|
|
if (c == 0x1) {
|
|
processMode = 2;
|
|
currentCount = 0;
|
|
startWhen = 0;
|
|
binEscape = false;
|
|
return;
|
|
}
|
|
// END of binary
|
|
else if (c == 0x3) {
|
|
// Record too small !
|
|
if (currentCount < sizeof(WII5_DATA_SpartonBinary)) {
|
|
serialSizeError++;
|
|
}
|
|
|
|
// Other validity
|
|
if (
|
|
(binRecords[currentRecord].channel != 0) // 0 - yep - valid
|
|
|| (binRecords[currentRecord].status != 105) // TODO 105, what is thta?
|
|
) {
|
|
// TODO Not really time, record data?
|
|
statsTimeError++;
|
|
}
|
|
|
|
// Status / Mode
|
|
lastValidRecord = currentRecord;
|
|
recordCount++;
|
|
processMode = 0;
|
|
|
|
// TODO consider logging time here for startWhen
|
|
return;
|
|
}
|
|
|
|
// Escape character - take off BIN next time
|
|
else if (c == 0x10) {
|
|
binEscape = true;
|
|
}
|
|
|
|
// Process a Binary row
|
|
else if (processMode == 2) {
|
|
if (binEscape) {
|
|
// Escape characters need to drop MSB ! (to protect 0x1, 0x3 and 0x10)
|
|
binEscape = false;
|
|
c = c & B01111111;
|
|
}
|
|
if (currentCount < sizeof(WII5_DATA_SpartonBinary)) {
|
|
((char*)&binRecords[currentRecord])[currentCount] = c;
|
|
currentCount++;
|
|
}
|
|
|
|
// Record too large
|
|
else {
|
|
serialSizeError++;
|
|
}
|
|
}
|
|
/*
|
|
TODO 2024 - passthrough mode?
|
|
else if (processMode == 0) {
|
|
console.log(LOG_DEBUG, F("SPARTON: char=%c"), c);
|
|
}
|
|
*/
|
|
}
|
|
|
|
void WII5Sparton::begin() {
|
|
// TODO Use ?
|
|
pinMode(SPARTON_RESET, OUTPUT);
|
|
digitalWrite(SPARTON_RESET, HIGH);
|
|
|
|
// Power (V2)
|
|
#if POWER_IMU_SPARTON_PIN>0
|
|
powerSetPin(POWER_IMU_SPARTON_PIN, POWER_IMU_SPARTON_ON);
|
|
powerOff(true); // Forced off at boot
|
|
#endif
|
|
|
|
debug = false;
|
|
running = false;
|
|
cancel = false;
|
|
step = WII5SPARTON_OFF;
|
|
|
|
consoleDirect = false;
|
|
rawCapture = false; // TODO NOTE: Debugging - probs should make this false by default
|
|
|
|
// Defaults
|
|
HZ = 8;
|
|
binMode = true;
|
|
|
|
// TODO - what should these be?
|
|
// recordCalc.setRejectionSigma(7.0);
|
|
|
|
};
|
|
|
|
uint16_t WII5Sparton::getHz() {
|
|
return HZ;
|
|
}
|
|
uint16_t WII5Sparton::getMs() {
|
|
return (1000 / getHz());
|
|
}
|
|
void WII5Sparton::setHz(uint16_t h) {
|
|
HZ = h;
|
|
}
|
|
void WII5Sparton::setBinary(bool in) {
|
|
binMode = in;
|
|
}
|
|
|
|
void WII5Sparton::setPassthrough(bool in) {} // {pt = in;}
|
|
bool WII5Sparton::getPassthrough() {} // {return pt;}
|
|
void WII5Sparton::setDebug(bool in) {debug = in;}
|
|
bool WII5Sparton::getDebug() {return debug;}
|
|
|
|
void WII5Sparton::setCapture(bool in) {
|
|
// TODO Capture disabled
|
|
capture = false; return;
|
|
// closeFile();
|
|
capture = in;
|
|
}
|
|
|
|
bool WII5Sparton::getCapture() {return capture;}
|
|
void WII5Sparton::setRecords(uint32_t in) { recordTotal = in; }
|
|
uint32_t WII5Sparton::getRecords() {return recordTotal;}
|
|
|
|
void WII5Sparton::automatic(uint8_t hz, uint32_t records, bool capture) {
|
|
setHz(hz);
|
|
setRecords(records);
|
|
// (leave as is) setPassthrough(0);
|
|
// Do not change debug
|
|
setCapture(capture);
|
|
start();
|
|
}
|
|
|
|
void WII5Sparton::start() {
|
|
console.log(LOG_DEBUG, F("SPARTON: start current=%d"), step);
|
|
|
|
pinMode(SPARTON_RESET, OUTPUT);
|
|
digitalWrite(SPARTON_RESET, HIGH);
|
|
|
|
if (step != WII5SPARTON_OFF) {
|
|
console.log(LOG_ERROR, F("SPARTON: ERROR - Start called when Sparton already running. Cleaning up."));
|
|
// closeFile();
|
|
sendLine(0);
|
|
off(true);
|
|
// TODO 2024 - delay? Consider safeDelay - although this is a rare occurecne (aka already running)
|
|
delay(500);
|
|
}
|
|
|
|
resetData();
|
|
step = WII5SPARTON_BOOT; stepWait = 0;
|
|
startWhen = 0; // Clear when binary start
|
|
startCapture = 0; // When we started this capture
|
|
lastWrite = 0; // Last time we wrote to SD Card successfully
|
|
// statsHighClear();
|
|
|
|
on(true);
|
|
}
|
|
|
|
void WII5Sparton::resetData() {
|
|
// Values used in collecting
|
|
serialSizeError = 0;
|
|
statsTimeError = 0;
|
|
|
|
blockNext = 0;
|
|
captureWriteMax = 0;
|
|
captureWriteMin = 1000;
|
|
captureWriteOver = 0;
|
|
|
|
// Current record.
|
|
#ifdef SPARTON_SIZES
|
|
for (uint8_t t = 0; t < SIZES_LENGTH; t++) {
|
|
sizes[t] = 0;
|
|
}
|
|
#endif
|
|
recordCount = 0; sendCount = 0;
|
|
|
|
// Averages
|
|
// avgAccelX.reset();
|
|
// avgAccelY.reset();
|
|
// avgAccelZ.reset();
|
|
// avgPoseX.reset();
|
|
// avgPoseY.reset();
|
|
// avgPoseZ.reset();
|
|
|
|
// Internal storage
|
|
#ifdef SPRTON_RECORD
|
|
recordReady = -1;
|
|
record[0].rec = 0; record[0].stamp = 0; record[0].stampActual = 0; record[0].stampErr = 0;
|
|
record[0].pose_x = NAN; record[0].pose_y = NAN; record[0].pose_z = NAN;
|
|
record[0].mag_x = NAN; record[0].mag_y = NAN; record[0].mag_z = NAN;
|
|
record[0].gyro_x = NAN; record[0].gyro_y = NAN; record[0].gyro_z = NAN;
|
|
record[0].accel_x = NAN; record[0].accel_y = NAN; record[0].accel_z = NAN;
|
|
record[1].rec = 0; record[0].stamp = 0; record[0].stampActual = 0; record[0].stampErr = 0;
|
|
record[1].pose_x = NAN; record[1].pose_y = NAN; record[1].pose_z = NAN;
|
|
record[1].mag_x = NAN; record[1].mag_y = NAN; record[1].mag_z = NAN;
|
|
record[1].gyro_x = NAN; record[1].gyro_y = NAN; record[1].gyro_z = NAN;
|
|
record[1].accel_x = NAN; record[1].accel_y = NAN; record[1].accel_z = NAN;
|
|
recordPrev.rec = 0; recordPrev.stamp = 0; recordPrev.stampActual = 0; recordPrev.stampErr = 0;
|
|
recordPrev.pose_x = NAN; recordPrev.pose_y = NAN; recordPrev.pose_z = NAN;
|
|
recordPrev.mag_x = NAN; recordPrev.mag_y = NAN; recordPrev.mag_z = NAN;
|
|
recordPrev.gyro_x = NAN; recordPrev.gyro_y = NAN; recordPrev.gyro_z = NAN;
|
|
recordPrev.accel_x = NAN; recordPrev.accel_y = NAN; recordPrev.accel_z = NAN;
|
|
#endif
|
|
|
|
console.log(LOG_INFO, F("SPARTON: Reset Sparton Data 0 - %d"), sizeof(binRecords));
|
|
memset(&binRecords, 0, sizeof(binRecords));
|
|
}
|
|
|
|
void WII5Sparton::info() {
|
|
if (debug)
|
|
console.log(LOG_DEBUG, F("SPARTON: info current=%d"), step);
|
|
recordCount = 0; sendCount = 0;
|
|
step = WII5SPARTON_INFO; stepWait = 0;
|
|
on();
|
|
}
|
|
|
|
void WII5Sparton::captureRun() {
|
|
// processMode = WII5SERIALPARSER_RECORD; // Capture fields
|
|
// last = WII5SERIALLAST_NONE;
|
|
step = WII5SPARTON_CAPTURE; stepWait = 0;
|
|
}
|
|
|
|
bool WII5Sparton::captureRunning() {
|
|
return (step == WII5SPARTON_CAPTURE);
|
|
}
|
|
|
|
bool WII5Sparton::captureFinished() {
|
|
return (
|
|
(step == WII5SPARTON_OFF)
|
|
);
|
|
}
|
|
|
|
// WAITING - It will wait here, basically forever, until you call captureRun()
|
|
// this is to allow preparation followed by remote trigger at a certain time.
|
|
// However - if it is past this - e.g. nowait - you could deadlock
|
|
bool WII5Sparton::captureWaiting() {
|
|
return (step == WII5SPARTON_PROGRAM_START);
|
|
}
|
|
|
|
void WII5Sparton::stop(bool force) {
|
|
if (debug)
|
|
console.log(LOG_DEBUG, F("SPARTON: cancel current=%d"), step);
|
|
if (step != WII5SPARTON_OFF) {
|
|
step = WII5SPARTON_CANCEL; stepWait = 0;
|
|
}
|
|
|
|
// Force to off state - done by sleep
|
|
if (force) {
|
|
powerOff(true);
|
|
step = WII5SPARTON_OFF;
|
|
}
|
|
}
|
|
|
|
void WII5Sparton::setRawCapture(bool in) {
|
|
rawCapture = in;
|
|
}
|
|
bool WII5Sparton::getRawCapture() {
|
|
return rawCapture;
|
|
}
|
|
|
|
void WII5Sparton::setConsoleDirect(bool in) {
|
|
consoleDirect = in;
|
|
}
|
|
bool WII5Sparton::getConsoleDirect() {
|
|
return consoleDirect;
|
|
}
|
|
|
|
bool WII5Sparton::isRunning() {
|
|
return running;
|
|
}
|
|
|
|
// on
|
|
void WII5Sparton::on(bool force) {
|
|
if (!running || force) {
|
|
if (debug)
|
|
console.log(LOG_DEBUG, F("SPARTON: power ON, baud=%lu"), (uint32_t)SerialIMU_Baud);
|
|
running = true;
|
|
cancel = false;
|
|
wii5Controller.shared5On();
|
|
#if POWER_IMU_SPARTON_PIN>0
|
|
powerOn();
|
|
#endif
|
|
// TODO: investigate whether pinMode(15, INPUT_PULLUP) was needed here
|
|
SerialIMU.begin(SerialIMU_Baud);
|
|
// SerialIMU.attachInterrupt(handleRxChar);
|
|
|
|
// stream = &SerialIMU;
|
|
// beginSerialManager();
|
|
}
|
|
}
|
|
|
|
void WII5Sparton::setBaudrate(uint32_t baud) {
|
|
console.log(LOG_DEBUG, F("SPARTON: Debug set baud=%lu"), baud);
|
|
SerialIMU.begin(baud);
|
|
}
|
|
|
|
// off
|
|
void WII5Sparton::off(bool force) {
|
|
if (running || force) {
|
|
if (debug)
|
|
console.log(LOG_DEBUG, F("SPARTON: power off"));
|
|
running = false;
|
|
SerialIMU.end();
|
|
// stream = NULL;
|
|
// endSerialManager();
|
|
displaySTATS();
|
|
#if POWER_IMU_SPARTON_PIN>0
|
|
powerOff();
|
|
#endif
|
|
wii5Controller.shared5Off();
|
|
}
|
|
}
|
|
|
|
// Sparton Things:
|
|
// - Turn Off
|
|
// - Start now
|
|
// - Start at X
|
|
|
|
bool firstBoot = true;
|
|
|
|
void WII5Sparton::sendLine(uint16_t l) {
|
|
programLine(l);
|
|
SerialIMU.println(wii5BufferString);
|
|
// TODO Configurable?
|
|
console.log(LOG_DEBUG, F("SPARTON: > %s"), wii5BufferString);
|
|
}
|
|
|
|
char c;
|
|
bool first;
|
|
uint8_t startReady;
|
|
void WII5Sparton::loop() {
|
|
first = (step != stepLast);
|
|
stepLast = step;
|
|
|
|
while (SerialIMU.available())
|
|
handleRxChar(SerialIMU.read());
|
|
|
|
// WII5SerialManager::loop();
|
|
switch (step) {
|
|
case WII5SPARTON_OFF:
|
|
off();
|
|
break;
|
|
|
|
case WII5SPARTON_BOOT:
|
|
on();
|
|
// Wait half a second then spin on
|
|
if (stepWait > 500) {
|
|
console.log(LOG_INFO, F("SPARTON: Startup"));
|
|
if (firstBoot) {
|
|
step = WII5SPARTON_BOOT_BAUD; stepWait = 0;
|
|
if (debug)
|
|
console.log(LOG_DEBUG, F("step boot->boot_baud"));
|
|
}
|
|
else {
|
|
step = WII5SPARTON_BOOT_STOP; stepWait = 0;
|
|
if (debug)
|
|
console.log(LOG_DEBUG, F("step boot->boot_stop"));
|
|
}
|
|
}
|
|
break;
|
|
|
|
// TODO Reset !!!
|
|
// - first - high, 500ms later, low, or reversed !
|
|
|
|
// First time boot - so lets check the baud rate is valid
|
|
case WII5SPARTON_BOOT_BAUD:
|
|
step = WII5SPARTON_BOOT_STOP; stepWait = 0;
|
|
if (debug)
|
|
console.log(LOG_DEBUG, F("step boot_baud->boot_stop"));
|
|
firstBoot = false;
|
|
break;
|
|
|
|
case WII5SPARTON_BOOT_STOP:
|
|
// First line is STOP
|
|
// processMode = WII5SERIALPARSER_NONE;
|
|
sendLine(0);
|
|
sendLine(0);
|
|
step = WII5SPARTON_BOOT_WAIT; stepWait = 0;
|
|
if (debug)
|
|
console.log(LOG_DEBUG, F("step boot_stop->boot_wait"));
|
|
break;
|
|
|
|
case WII5SPARTON_BOOT_WAIT:
|
|
// First time - clear the captture file
|
|
// Give it a full second to absorb all the junk
|
|
if (stepWait > 1000) {
|
|
SerialIMU.flush();
|
|
// processMode = WII5SERIALPARSER_NONE;
|
|
step = WII5SPARTON_READY; stepWait = 0;
|
|
if (debug)
|
|
console.log(LOG_DEBUG, F("step boot_wait->ready"));
|
|
}
|
|
break;
|
|
|
|
case WII5SPARTON_READY:
|
|
// Short Params (no gyro or mag)
|
|
programStart = 20; programEnd = 30; sendCount = 0;
|
|
// Experiment
|
|
// programStart = 50; programEnd = 60; sendCount = 0;
|
|
step = WII5SPARTON_PROGRAM_BASE; stepWait = 0;
|
|
if (debug)
|
|
console.log(LOG_DEBUG, F("step ready->program_base"));
|
|
break;
|
|
|
|
case WII5SPARTON_PROGRAM_BASE:
|
|
if (stepWait > 250) {
|
|
if (sendCount <= (programEnd - programStart)) {
|
|
// Send next line, inremement counter, and wait another 10 ms
|
|
sendLine(programStart + sendCount);
|
|
sendCount++;
|
|
stepWait = 0;
|
|
}
|
|
else {
|
|
step = WII5SPARTON_PROGRAM_START; stepWait = 0;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case WII5SPARTON_PROGRAM_START:
|
|
// // processMode = WII5SERIALPARSER_RECORD; // Capture fields
|
|
// // last = WII5SERIALLAST_NONE;
|
|
// step = WII5SPARTON_CAPTURE; stepWait = 0;
|
|
// Sits here, waiting forever, until we move it on manually when ready to run.
|
|
// But we stillshould add a timeout
|
|
/// SKIPT to capture
|
|
if (true) {
|
|
// processMode = WII5SERIALPARSER_RECORD; // Capture fields
|
|
// last = WII5SERIALLAST_NONE;
|
|
step = WII5SPARTON_CAPTURE; stepWait = 0;
|
|
}
|
|
break;
|
|
|
|
case WII5SPARTON_CAPTURE:
|
|
// Reset counters, so as not to loose first lot
|
|
if (first) {
|
|
console.log(LOG_INFO, F("SPARTON: Capturing"));
|
|
resetData();
|
|
timeStart = now();
|
|
}
|
|
|
|
// We do everything else in the gap between the records (TBC)
|
|
// TODO This has not been tested
|
|
else if (startWhen > 5) {
|
|
|
|
// TODO Fix hardcoded time 5 above
|
|
|
|
// TODO Fix hard coded 15 recods per block
|
|
|
|
// Do we need to wrtie a block of data
|
|
if ( (recordCount / 15) > blockNext) {
|
|
|
|
// TODO test if block too far
|
|
if ( (recordCount / 15) > (blockNext + 1) ) {
|
|
console.log(LOG_FATAL, F("Sparton: Block too far, all data probably lost. block=%lu, expected=%lu, records=%lu"),
|
|
blockNext,
|
|
recordCount / 15,
|
|
recordCount
|
|
);
|
|
}
|
|
|
|
captureWriteTime = 0;
|
|
if (sdBlock.dataIsOpen()) {
|
|
if (sdBlock.dataWrite(
|
|
&binRecords[(blockNext % 2) * 15],
|
|
15 * 33
|
|
)) {
|
|
lastWrite = 0;
|
|
// TODO No, slow
|
|
/*
|
|
console.log(LOG_INFO, F("Sparton: Block record write OK: block=%lu, records=%lu, binRecords=%d, size=%d"),
|
|
blockNext, recordCount,
|
|
int(&binRecords[(blockNext % 2) * 15]),
|
|
int(15 * 33)
|
|
);
|
|
*/
|
|
}
|
|
else {
|
|
// TODO No, slow
|
|
console.log(LOG_ERROR, F("Sparton: Block record write FAILED: %lu"), blockNext);
|
|
// At this point - we stop processing ! But need to log that.
|
|
console.log(LOG_ERROR, F("Sparton: CANCEL CAPTURE - Can't write to SD Card"));
|
|
stop();
|
|
}
|
|
}
|
|
else {
|
|
console.log(LOG_ERROR, F("Sparton: Block Data not open"));
|
|
// At this point - we stop processing ! But need to log that.
|
|
console.log(LOG_ERROR, F("Sparton: CANCEL CAPTURE - Can't write to SD Card"));
|
|
stop();
|
|
}
|
|
|
|
if (captureWriteTime > captureWriteMax)
|
|
captureWriteMax = captureWriteTime;
|
|
if (captureWriteTime < captureWriteMin)
|
|
captureWriteMin = captureWriteTime;
|
|
if (captureWriteTime > 5)
|
|
captureWriteOver++;
|
|
|
|
// TODO Catch errors?
|
|
|
|
blockNext++;
|
|
}
|
|
|
|
else if ((stepWait > 700) && ((recordCount % HZ) == 0) && (lastValidRecord < SPARTON_RECORDS) ) {
|
|
// console.printBinary((char*)&binBuffer[binBufferReady], sizeof(binBuffer[0]));
|
|
wii5Display.atDataSend(
|
|
// NOTE: Must not add the "@" - it isn't used in CRC calculation
|
|
// line,stamp,err,8000,0,0,accel:x,y,z,gyro:x,y,z,mag:x,y,z,pos:x,y,z
|
|
F("CaptureSummary"),
|
|
F("%lu/%lu,%d,%d,%d,%d,%d,%d,block=%lu/%lu"),
|
|
recordCount, recordTotal,
|
|
int(binRecords[lastValidRecord].accel_x),
|
|
int(binRecords[lastValidRecord].accel_y),
|
|
int(binRecords[lastValidRecord].accel_z),
|
|
int(binRecords[lastValidRecord].pose_x),
|
|
int(binRecords[lastValidRecord].pose_y),
|
|
int(binRecords[lastValidRecord].pose_z),
|
|
blockNext, (recordTotal / 15)
|
|
);
|
|
/*
|
|
Not recommended - 1 per second at 9600
|
|
wii5Display.statusSend(
|
|
F("Sparton"),
|
|
F("record=%lu/%lu,z=%d"),
|
|
recordCount, recordTotal,
|
|
int(binRecords[lastValidRecord].accel_z)
|
|
);
|
|
*/
|
|
stepWait = 0;
|
|
|
|
}
|
|
|
|
// New 2024 - Check if recorsds never increasing and report errors !
|
|
// No records but startTime > 2 minutes - FAILURE !
|
|
// TODO 2024 - This does NOT capture if the sparton fails half way through....
|
|
// What we want is no records for more than 2 minutes.
|
|
// Change this code so startCapture is something like lastWriteTime or lastRecordTime and check that isn't 2 minutes old
|
|
else if ( lastWrite > 120000) {
|
|
console.log(LOG_INFO, F("Sparton: Failure ! No writes for 2 minutes"));
|
|
snprintf_P(
|
|
wii5Commands.lastCommandMessage, sizeof(wii5Commands.lastCommandMessage),
|
|
PSTR("@Err,sparton,norecords")
|
|
);
|
|
wii5Commands.lastCommandCmd = 0;
|
|
wii5Communications.sendError();
|
|
step = WII5SPARTON_PROGRAM_STOP; stepWait = 0;
|
|
}
|
|
|
|
// Do we have enough?
|
|
else if (recordCount >= recordTotal) {
|
|
// If we are new block. OR half way through a block...
|
|
if (
|
|
sdBlock.dataIsOpen()
|
|
&& ((recordCount / 15) >= blockNext)
|
|
&& ((recordCount % 15) > 0)
|
|
) {
|
|
|
|
// If half way, blank the rest
|
|
if ((recordCount % 15) > 0) {
|
|
// Blank the other records - 0 or 15 start + left over to end
|
|
memset(&binRecords[((blockNext % 2) * 15) + (recordCount % 15)], 0, 33 * (15 - (recordCount % 15)));
|
|
}
|
|
|
|
if (!sdBlock.dataWrite(
|
|
&binRecords[(blockNext % 2) * 15],
|
|
15 * 33
|
|
)) {
|
|
console.log(LOG_ERROR, F("Sparton: Block record write FAILED: %lu"), blockNext);
|
|
}
|
|
|
|
blockNext++;
|
|
}
|
|
|
|
timeEnd = now();
|
|
console.log(LOG_INFO, F("SPARTON: Finished Capture. Records=%lu of %lu, block written=%lu"), recordCount, recordTotal, blockNext);
|
|
// processMode = WII5SERIALPARSER_NONE;
|
|
// last = WII5SERIALLAST_NONE;
|
|
// Display high speed stats
|
|
// statsHighDisplay();
|
|
if (debug)
|
|
console.log(LOG_DEBUG, F("step capture->program_stop"));
|
|
step = WII5SPARTON_PROGRAM_STOP; stepWait = 0;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case WII5SPARTON_PROGRAM_STOP:
|
|
// TODO send the stop command
|
|
// programStart = 0; programEnd = 1;
|
|
// step = WII5SPARTON_PROGRAM_BASE; stepWait = 0;
|
|
// processMode = WII5SERIALPARSER_NONE;
|
|
sendLine(0);
|
|
step = WII5SPARTON_FINISH; stepWait = 0;
|
|
// last = WII5SERIALLAST_NONE;
|
|
if (debug)
|
|
console.log(LOG_DEBUG, F("step program_stop->finish"));
|
|
break;
|
|
|
|
case WII5SPARTON_CANCEL:
|
|
// Shut it down., power will go down at OFF
|
|
// closeFile();
|
|
sendLine(0);
|
|
step = WII5SPARTON_FINISH; stepWait = 0;
|
|
cancel = true;
|
|
if (debug)
|
|
console.log(LOG_DEBUG, F("step cancel->finish"));
|
|
break;
|
|
|
|
case WII5SPARTON_FINISH:
|
|
step = WII5SPARTON_OFF; stepWait = 0;
|
|
if (debug)
|
|
console.log(LOG_DEBUG, F("step finish->off"));
|
|
break;
|
|
|
|
// TODO DEBUG THIS !
|
|
case WII5SPARTON_INFO:
|
|
if (first) {
|
|
sendCount = 0;
|
|
}
|
|
else if (stepWait > 500) {
|
|
// Standard setup commands
|
|
if (sendCount <= 12) {
|
|
// Send next line, inremement counter, and wait another 10 ms
|
|
sendLine(sendCount);
|
|
sendCount++;
|
|
stepWait = 0;
|
|
}
|
|
|
|
// 13 = 30 = Version
|
|
else if (sendCount == 13) {
|
|
sendLine(80);
|
|
sendCount++;
|
|
stepWait = 0;
|
|
}
|
|
|
|
// 14 = 31 = Config XML
|
|
else if (sendCount == 14) {
|
|
if (stepWait > 3000) {
|
|
sendLine(81);
|
|
sendCount++;
|
|
stepWait = 0;
|
|
}
|
|
}
|
|
|
|
// Wait 5 seconds to end
|
|
else if (stepWait > 60000) {
|
|
step = WII5SPARTON_OFF; stepWait = 0;
|
|
}
|
|
}
|
|
break;
|
|
|
|
default:
|
|
console.log(LOG_FATAL, F("SPARTON: Default step... step=%d"), step);
|
|
step = WII5SPARTON_OFF;
|
|
// Power off
|
|
break;
|
|
}
|
|
|
|
while (SerialIMU.available())
|
|
handleRxChar(SerialIMU.read());
|
|
|
|
return;
|
|
}
|
|
|
|
void WII5Sparton::repl() {
|
|
uint8_t lastDot = 0;
|
|
console.log(LOG_FATAL, F("SPARTON: REPL. '...' to exit."));
|
|
|
|
start();
|
|
// SerialIMU.detachInterrupt();
|
|
while (1) {
|
|
if (SerialConsole.available()) {
|
|
char c = SerialConsole.read();
|
|
if (c == '.')
|
|
lastDot++;
|
|
else
|
|
lastDot = 0;
|
|
if (lastDot >= 3) {
|
|
SerialConsole.flush();
|
|
SerialIMU.flush();
|
|
stop();
|
|
console.log(LOG_FATAL, F("SPARTON: Completed REPL. User exit."));
|
|
return;
|
|
}
|
|
SerialIMU.write(c);
|
|
}
|
|
|
|
if (SerialIMU.available()) {
|
|
char c = SerialIMU.read() ;
|
|
SerialConsole.write(c);
|
|
}
|
|
}
|
|
}
|
|
|
|
void WII5Sparton::displaySTATS() {
|
|
wii5Display.atStatsSend(
|
|
F("Sparton"),
|
|
F("errorTime=%lu,errorSize=%lu,writeMax=%lu,writeMin=%lu,writeOver=%lu"),
|
|
statsTimeError, serialSizeError,
|
|
captureWriteMax, captureWriteMin, captureWriteOver
|
|
);
|
|
#ifdef SPARTON_SIZES
|
|
for (uint8_t t = 0; t < SIZES_LENGTH; t++) {
|
|
if (sizes[t] > 0) {
|
|
if (t == 0)
|
|
console.printf(F("# SPARTON Record Size: others = %d\r\n"), sizes[t]);
|
|
else
|
|
console.printf(F("# SPARTON Record Size: %d = %d\r\n"), t + SIZES_START, sizes[t]);
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
|
|
// TODO Totaly - this can be common wii5BufferString for all Serial Devices or even the Console
|
|
void WII5Sparton::programLine(uint16_t l) {
|
|
memset(wii5BufferString, 0, WII5_BUFFER_STRING);
|
|
|
|
// Main Programming
|
|
switch (l) {
|
|
|
|
// CODE = Wave Tank and original code using all things, including magnatometer
|
|
// PROGRAM=0..12, START=13..13, PROGRAM+START=0..13, STOP=0..0
|
|
case 0:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0TriggerDivisor 0 set drop"));
|
|
break;
|
|
case 1:
|
|
// TODO: confirm whether the reset+wait sequence here is actually required
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("accelrange 2 set drop"));
|
|
break;
|
|
case 2:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0Enables array[ 0 15 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]array set drop"));
|
|
break;
|
|
case 3:
|
|
if (binMode)
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0Format 2 set drop"));
|
|
else
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0Format 3 set drop"));
|
|
break;
|
|
case 4:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit cputime dvid@ set drop"));
|
|
break;
|
|
case 5:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit magp dvid@ set drop"));
|
|
break;
|
|
case 6:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit accelp dvid@ set drop"));
|
|
break;
|
|
case 7:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit roll dvid@ set drop"));
|
|
break;
|
|
case 8:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit pitch dvid@ set drop"));
|
|
break;
|
|
case 9:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit yaw dvid@ set drop"));
|
|
break;
|
|
case 10:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit gyrop dvid@ set drop"));
|
|
break;
|
|
case 11:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0Trigger 6 set drop"));
|
|
break;
|
|
case 12:
|
|
if (HZ == 64) {
|
|
// 64Hz
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 156 set drop"));
|
|
console.log(LOG_DEBUG, F("HZ 64, setting 100us timer to 156"));
|
|
}
|
|
else if (HZ == 8) {
|
|
// 8Hz
|
|
// TODO
|
|
// strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 10000 set drop"));
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 1250 set drop"));
|
|
console.log(LOG_DEBUG, F("HZ 8, setting 100us timer to 1250"));
|
|
}
|
|
else if (HZ == 1) {
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 10000 set drop"));
|
|
console.log(LOG_DEBUG, F("HZ 8, setting 100us timer to 1250"));
|
|
}
|
|
else {
|
|
console.log(LOG_FATAL, F("Invalid HZ for Sprton = %d"), HZ);
|
|
}
|
|
break;
|
|
case 13:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0TriggerDivisor 1 set drop"));
|
|
break;
|
|
|
|
// CODE = Cut down code for WII5.1 Tas Delivery - no Gyro or Mag
|
|
// PROGRAM=20..29
|
|
case 20:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0TriggerDivisor 0 set drop"));
|
|
break;
|
|
case 21:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0Enables array[ 0 15 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]array set drop"));
|
|
break;
|
|
case 22:
|
|
if (binMode)
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0Format 2 set drop"));
|
|
else
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0Format 3 set drop"));
|
|
break;
|
|
case 23:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit cputime dvid@ set drop"));
|
|
break;
|
|
case 24:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit accelp dvid@ set drop"));
|
|
break;
|
|
case 25:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit roll dvid@ set drop"));
|
|
break;
|
|
case 26:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit pitch dvid@ set drop"));
|
|
break;
|
|
case 27:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit yaw dvid@ set drop"));
|
|
break;
|
|
case 28:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0Trigger 6 set drop"));
|
|
break;
|
|
case 29:
|
|
if (HZ == 64) {
|
|
// 64Hz
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 156 set drop"));
|
|
console.log(LOG_DEBUG, F("HZ 64, setting 100us timer to 156"));
|
|
}
|
|
else if (HZ == 8) {
|
|
// 8Hz
|
|
// TODO
|
|
// strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 10000 set drop"));
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 1250 set drop"));
|
|
console.log(LOG_DEBUG, F("HZ 8, setting 100us timer to 1250"));
|
|
}
|
|
else if (HZ == 1) {
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 10000 set drop"));
|
|
console.log(LOG_DEBUG, F("HZ 8, setting 100us timer to 1250"));
|
|
}
|
|
else {
|
|
console.log(LOG_FATAL, F("Invalid HZ for Sprton = %d"), HZ);
|
|
}
|
|
break;
|
|
case 30:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0TriggerDivisor 1 set drop"));
|
|
break;
|
|
|
|
// PROGRAM=0..12, START=13..13, PROGRAM+START=0..13, STOP=0..0
|
|
case 50:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0TriggerDivisor 0 set drop"));
|
|
break;
|
|
case 51:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("accelrange 2 set drop"));
|
|
break;
|
|
case 52:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0Enables array[ 0 15 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]array set drop"));
|
|
break;
|
|
case 53:
|
|
if (binMode)
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0Format 2 set drop"));
|
|
else
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0Format 3 set drop"));
|
|
break;
|
|
case 54:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit cputime dvid@ set drop"));
|
|
break;
|
|
case 55:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit roll dvid@ set drop"));
|
|
break;
|
|
case 56:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit pitch dvid@ set drop"));
|
|
break;
|
|
case 57:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0EnableBit yaw dvid@ set drop"));
|
|
break;
|
|
case 58:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0Trigger 6 set drop"));
|
|
break;
|
|
case 59:
|
|
if (HZ == 64) {
|
|
// 64Hz
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 156 set drop"));
|
|
console.log(LOG_DEBUG, F("HZ 64, setting 100us timer to 156"));
|
|
}
|
|
else if (HZ == 8) {
|
|
// 8Hz
|
|
// TODO
|
|
// strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 10000 set drop"));
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 1250 set drop"));
|
|
console.log(LOG_DEBUG, F("HZ 8, setting 100us timer to 1250"));
|
|
}
|
|
else if (HZ == 1) {
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chanTimerX100us 10000 set drop"));
|
|
console.log(LOG_DEBUG, F("HZ 8, setting 100us timer to 1250"));
|
|
}
|
|
else {
|
|
console.log(LOG_FATAL, F("Invalid HZ for Sprton = %d"), HZ);
|
|
}
|
|
break;
|
|
case 60:
|
|
strcpy_P(wii5BufferString ,(PGM_P) F("chan0TriggerDivisor 1 set drop"));
|
|
break;
|
|
|
|
|
|
// Debugging / Logging
|
|
// 14-20
|
|
case 80:
|
|
strcpy_PF(wii5BufferString ,F("chan0TriggerDivisor 0 set drop"));
|
|
break;
|
|
case 81:
|
|
strcpy_P(wii5BufferString, (PGM_P) F("name di."));
|
|
break;
|
|
case 82:
|
|
strcpy_P(wii5BufferString, (PGM_P) F("serialnumber di."));
|
|
break;
|
|
case 83:
|
|
strcpy_P(wii5BufferString, (PGM_P) F("VERSION di."));
|
|
break;
|
|
case 84:
|
|
strcpy_P(wii5BufferString, (PGM_P) F("VERSION_M4 di."));
|
|
break;
|
|
case 85:
|
|
strcpy_P(wii5BufferString, (PGM_P) F("gyroSampleRate di."));
|
|
break;
|
|
case 86:
|
|
strcpy_P(wii5BufferString, (PGM_P) F("save_mask d.on"));
|
|
break;
|
|
case 87:
|
|
strcpy_P(wii5BufferString, (PGM_P) F("db.print"));
|
|
break;
|
|
|
|
case 90:
|
|
strcpy_P(wii5BufferString, (PGM_P) F("VERSION di."));
|
|
break;
|
|
|
|
case 91:
|
|
strcpy_P(wii5BufferString, (PGM_P) F("chan0Enable."));
|
|
break;
|
|
|
|
case 100:
|
|
strcpy_P(wii5BufferString, (PGM_P) F("baud 8 set drop"));
|
|
break;
|
|
|
|
default:
|
|
console.log(LOG_FATAL, F("Sparton: request invalid programming string"));
|
|
wii5BufferString[0] = '\0';
|
|
break;
|
|
}
|
|
}
|
|
|
|
WII5Sparton wii5Sparton;
|