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.
287 lines
7.0 KiB
C++
287 lines
7.0 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.h
|
|
* @brief Sparton AHRS-M1/M2 IMU driver: capture timing, NorthTek configuration.
|
|
*/
|
|
|
|
#ifndef WII5Sparton_h
|
|
#define WII5Sparton_h
|
|
|
|
#include <Arduino.h>
|
|
#include <WII5.h>
|
|
#include <elapsedMillis.h>
|
|
#include <TimeLib.h>
|
|
#include <AvgStd.h>
|
|
|
|
#define SPARTON_RECORDS 30
|
|
|
|
/*
|
|
|
|
Sparton format
|
|
|
|
// $CUS0,
|
|
// POSE * 1000
|
|
// 5.007294e-01,-3.079974,294.518433,
|
|
// MAG * 1000
|
|
// 70.337479,174.480072,-450.354431,
|
|
// ACCEL * 10,000
|
|
// 3.233985,-53.680248,1007.986023,
|
|
// GYRO * 1000
|
|
// 2.710441e-02,-2.003428e-02,7.615486e-02,
|
|
// 75988968,*
|
|
|
|
float pose_x;
|
|
float pose_y;
|
|
float pose_z;
|
|
float mag_x;
|
|
float mag_y;
|
|
float mag_z;
|
|
float accel_x;
|
|
float accel_y;
|
|
float accel_z;
|
|
float gyro_x;
|
|
float gyro_y;
|
|
float gyro_z;
|
|
uint32_t stamp;
|
|
|
|
chan0Enable.
|
|
<item name="pitch">
|
|
<vid>8</vid>
|
|
<offset>0</offset>
|
|
<type>Float32</type>
|
|
</item>
|
|
<item name="roll">
|
|
<vid>9</vid>
|
|
<offset>4</offset>
|
|
<type>Float32</type>
|
|
</item>
|
|
<item name="yaw">
|
|
<vid>10</vid>
|
|
<offset>8</offset>
|
|
<type>Float32</type>
|
|
</item>
|
|
<item name="magp">
|
|
<vid>23</vid>
|
|
<offset>12</offset>
|
|
<type>Float32[3]</type>
|
|
</item>
|
|
<item name="accelp">
|
|
<vid>25</vid>
|
|
<offset>24</offset>
|
|
<type>Float32[3]</type>
|
|
</item>
|
|
<item name="gyrop">
|
|
<vid>27</vid>
|
|
<offset>36</offset>
|
|
<type>Float32[3]</type>
|
|
</item>
|
|
<item name="cputime">
|
|
<vid>249</vid>
|
|
<offset>48</offset>
|
|
<type>Int32</type>
|
|
</item>
|
|
|
|
|
|
* 52 = Bytes of record size
|
|
* But records appear to be about 70+ Bytes
|
|
|
|
New Record Types:
|
|
|
|
|
|
*/
|
|
|
|
enum WII5SPARTON_STEPS {
|
|
WII5SPARTON_OFF,
|
|
WII5SPARTON_BOOT,
|
|
WII5SPARTON_BOOT_BAUD,
|
|
WII5SPARTON_BOOT_STOP,
|
|
WII5SPARTON_BOOT_WAIT,
|
|
WII5SPARTON_READY,
|
|
WII5SPARTON_PROGRAM_BASE,
|
|
WII5SPARTON_PROGRAM_START,
|
|
WII5SPARTON_CAPTURE,
|
|
WII5SPARTON_PROGRAM_STOP,
|
|
WII5SPARTON_INFO,
|
|
WII5SPARTON_INFO_WAIT,
|
|
WII5SPARTON_CANCEL,
|
|
WII5SPARTON_FINISH
|
|
};
|
|
|
|
/**
|
|
* @brief Driver for the Sparton AHRS-M1 / AHRS-M2 IMU.
|
|
*
|
|
* Programs the AHRS via NorthTek-style commands over a UART, captures
|
|
* binary records (pose / mag / accel / gyro / cputime) at a configured
|
|
* sample rate, and writes them either straight to SD or via the console
|
|
* passthrough. State machine in WII5SPARTON_STEPS; record layout in
|
|
* WII5_DATA_SpartonBinary.
|
|
*/
|
|
class WII5Sparton : public WII5Power {
|
|
public:
|
|
WII5Sparton() {}
|
|
virtual WII5_CONTROLLERS controllerId() {return WII5CONTROLLER_DRIVER;}
|
|
virtual WII5_DRIVERS driverId() {return WII5DRIVER_SPARTON;}
|
|
|
|
/** @brief One-time bring-up. */
|
|
void begin();
|
|
/** @brief State-machine tick: parses incoming records, advances steps. */
|
|
void loop();
|
|
|
|
/** @brief Begin a programming + capture cycle. */
|
|
virtual void start();
|
|
/** @brief Query the AHRS for firmware/version info. */
|
|
void info();
|
|
/** @brief Abort the current cycle. */
|
|
void stop(bool force = false);
|
|
|
|
/** @brief Run an automated capture for `records` samples at `hz` Hz. */
|
|
void automatic(uint8_t hz, uint32_t records, bool capture);
|
|
|
|
/** @brief Reconfigure the IMU UART baud rate. */
|
|
virtual void setBaudrate(uint32_t baud);
|
|
|
|
/** @brief Apply power to the Sparton. */
|
|
void on(bool forced = false);
|
|
/** @brief Cut power to the Sparton. */
|
|
void off(bool forced = false);
|
|
|
|
/** @brief Is a programming/capture cycle in flight? */
|
|
bool isRunning();
|
|
/** @brief Begin a capture (after programming completes). */
|
|
void captureRun();
|
|
/** @brief Are we currently capturing samples? */
|
|
bool captureRunning();
|
|
/** @brief Are we waiting for a capture trigger? */
|
|
bool captureWaiting();
|
|
/** @brief Has the most recent capture completed? */
|
|
bool captureFinished();
|
|
bool capture;
|
|
|
|
time_t lastStartTime;
|
|
uint32_t lastRunTime;
|
|
|
|
/** @brief Send a single NorthTek programming line to the IMU. */
|
|
void sendLine(uint16_t l);
|
|
|
|
/** @brief Enable/disable raw-record passthrough to the capture file. */
|
|
void setRawCapture(bool in);
|
|
/** @brief Current raw-capture flag. */
|
|
bool getRawCapture();
|
|
|
|
/** @brief Enable/disable console-direct output (used when no SD card is present). */
|
|
void setConsoleDirect(bool in);
|
|
/** @brief Current console-direct flag. */
|
|
bool getConsoleDirect();
|
|
/** @brief Print capture statistics to the console. */
|
|
virtual void displaySTATS();
|
|
|
|
/** @brief Current sample rate in Hz. */
|
|
uint16_t getHz();
|
|
/** @brief Current sample period in milliseconds. */
|
|
uint16_t getMs();
|
|
/** @brief Reconfigure the sample rate (Hz). */
|
|
void setHz(uint16_t h);
|
|
/** @brief Enable/disable binary record format. */
|
|
void setBinary(bool in);
|
|
|
|
/** @brief Emit a single NorthTek programming line by index. */
|
|
virtual void programLine(uint16_t l);
|
|
|
|
/** @brief Enable/disable raw serial passthrough to the console. */
|
|
void setPassthrough(bool in);
|
|
/** @brief Current passthrough flag. */
|
|
bool getPassthrough();
|
|
/** @brief Enable/disable verbose debug output. */
|
|
void setDebug(bool in);
|
|
/** @brief Current debug flag. */
|
|
bool getDebug();
|
|
/** @brief Enable/disable capture-on-start. */
|
|
void setCapture(bool in);
|
|
/** @brief Current capture flag. */
|
|
bool getCapture();
|
|
/** @brief Set the number of records to capture. */
|
|
void setRecords(uint32_t in);
|
|
/** @brief Current record count target. */
|
|
uint32_t getRecords();
|
|
|
|
/** @brief Drop into a NorthTek REPL for interactive configuration. */
|
|
void repl();
|
|
|
|
/** @brief Reset internal capture buffers and counters. */
|
|
void resetData();
|
|
|
|
// TODO int ? float, long ?
|
|
/*
|
|
int lastAccelX;
|
|
int lastAccelY;
|
|
int lastAccelZ;
|
|
int lastPoseX;
|
|
int lastPoseY;
|
|
int lastPoseZ;
|
|
*/
|
|
|
|
// Last capture errors etc
|
|
uint32_t captureWriteMax;
|
|
uint32_t captureWriteMin;
|
|
uint32_t captureWriteOver;
|
|
uint32_t statsTimeError;
|
|
uint32_t serialSizeError;
|
|
|
|
protected:
|
|
bool running;
|
|
bool cancel;
|
|
bool rawCapture; // Do we want to write raw data to capture file
|
|
bool consoleDirect; // Direct access
|
|
uint16_t HZ;
|
|
bool binMode;
|
|
bool debug;
|
|
|
|
uint32_t recordTotal;
|
|
elapsedMillis elapsedRequest;
|
|
|
|
// Inclusive
|
|
uint32_t sendCount; // Where are we up to in program data
|
|
uint16_t programStart;
|
|
uint16_t programEnd;
|
|
|
|
// Internal steps
|
|
WII5SPARTON_STEPS step;
|
|
WII5SPARTON_STEPS stepLast;
|
|
elapsedMillis stepWait;
|
|
|
|
void WII5Sparton::handleRxChar( uint8_t c );
|
|
|
|
uint32_t recordCount;
|
|
uint8_t lastValidRecord;
|
|
|
|
// Binary Records
|
|
WII5_DATA_SpartonBinary binRecords[SPARTON_RECORDS];
|
|
uint8_t currentRecord;
|
|
uint8_t currentCount;
|
|
|
|
bool binEscape;
|
|
uint8_t processMode;
|
|
|
|
|
|
elapsedMillis startCapture;
|
|
elapsedMillis startWhen;
|
|
elapsedMillis lastWrite;
|
|
// elapsedMillis endWhen = 0;
|
|
uint32_t blockNext;
|
|
|
|
elapsedMillis captureWriteTime;
|
|
|
|
time_t timeStart;
|
|
time_t timeEnd;
|
|
|
|
};
|
|
|
|
extern WII5Sparton wii5Sparton;
|
|
|
|
#endif
|