Skip to content

Commit

Permalink
State machines, Controllers, and use EEPROM flashing in ACEINNA module
Browse files Browse the repository at this point in the history
ryodine committed May 31, 2020
1 parent 1f6d711 commit c76eb34
Showing 16 changed files with 628 additions and 185 deletions.
70 changes: 48 additions & 22 deletions ACEINNAInclinometer.cpp
Original file line number Diff line number Diff line change
@@ -1,38 +1,50 @@
#include "ACEINNAInclinometer.h"

#include "CANInterface.h"
#include "FaultHandling.h"

//! UNCOMMENT BELOW IF YOU WANT TO SET UP THE ACEINNA INCLINOMETER (FIRST TIME)
//#define PROVISION_CAN_ACEINNA_MODULE

bool Inclinometer::ACEINNAInclinometer::begin()
{
canInterface.begin(CAN::SerialBaudrate::baud_115200,
CAN::CANBusBaudrate::kbps_250);

#ifdef PROVISION_CAN_ACEINNA_MODULE
// Set Output data rate to 10Hz (10=10Hz; 20=5Hz)
canInterface.write(
CAN::J1939Message(k_sourceAddress, k_aceinnaAddress, PGN_ODR, 10),
k_aceinnaAddress); // Set Output data rate to 10Hz (10=10Hz; 20=5Hz)
k_aceinnaAddress);

// Only use SSI2 (Inclination) data
canInterface.write(CAN::J1939Message(k_sourceAddress, k_aceinnaAddress,
PGN_PERIODIC_DATA_TYPES, 1),
k_aceinnaAddress); // Only use SSI2 (Inclination) data
canInterface.write(
CAN::J1939Message(k_sourceAddress, k_aceinnaAddress, PGN_LOW_PASS, 2),
k_aceinnaAddress); // 2Hz digital low pass filter
delay(100);
k_aceinnaAddress);

// 2Hz digital low pass filter
canInterface.write(CAN::J1939Message(k_sourceAddress, k_aceinnaAddress,
PGN_LOW_PASS, 2, 2),
k_aceinnaAddress);

// TODO: Attempt to communicate with the incinometer and return false if it
// doesnt work
// Save config to EEPROM
canInterface.write(CAN::J1939Message(k_sourceAddress, k_aceinnaAddress,
PGN_SAVE_EEPROM, 0, k_aceinnaAddress,
1),
k_aceinnaAddress);
#endif

// flush the buffer
canInterface.flushBuffer();
return true;
delay(300);
if (hasData())
return true;

return false;
}
bool Inclinometer::ACEINNAInclinometer::hasData()
{
//TODO: pre-read and check if there is SSI2 data, then cache it!
return canInterface.hasPacket();
}
Eigen::Vector2d Inclinometer::ACEINNAInclinometer::getData()
{
if (hasData()) {
// TODO: pre-read and check if there is SSI2 data, then cache it!
if (canInterface.hasPacket()) {
CAN::J1939Message m = canInterface.read();
canInterface
.flushBuffer(); // This will ensure that we aren't reading
@@ -52,12 +64,26 @@ Eigen::Vector2d Inclinometer::ACEINNAInclinometer::getData()
double pitch_adjusted = (pitch * (1.0 / 32768) - 250.0);
double roll_adjusted = (roll * (1.0 / 32768) - 250.0);

return Eigen::Vector2d(pitch_adjusted * PI / 180.0,
roll_adjusted * PI / 180.0);
if (pitch_adjusted > k_anglePlausibilityRange ||
pitch_adjusted < -k_anglePlausibilityRange ||
roll_adjusted > k_anglePlausibilityRange ||
roll_adjusted < -k_anglePlausibilityRange) {
Fault::Handler::instance()->setFaultCode(
Fault::INCLINOMETER_IMPLAUSIBLE_READING);
}

cachedAngles = Eigen::Vector2d(pitch_adjusted * PI / 180.0,
roll_adjusted * PI / 180.0);
hasDataCached = true;
return true;
}
Serial.print("PGN was actually: ");
Serial.println(m.CanID.getPGN());
}

return Eigen::Vector2d(-100.0, 100.0);
return false;
}
Eigen::Vector2d Inclinometer::ACEINNAInclinometer::getData()
{
hasDataCached = false;
roll.addPoint(cachedAngles[0]);
pitch.addPoint(cachedAngles[1]);
return Eigen::Vector2d(roll.getAverage(), pitch.getAverage());
}
48 changes: 32 additions & 16 deletions ACEINNAInclinometer.h
Original file line number Diff line number Diff line change
@@ -15,6 +15,7 @@

#include "CANSAEJ1939.h"
#include "InclinometerInterface.h"
#include "MovingAverage.h"

#include <Arduino.h>

@@ -30,9 +31,13 @@ class ACEINNAInclinometer : public InclinometerDataSource {
*
* @param canSerialInterface the HardwareSerial interface on the Arduino
* that is connected to the CAN Bus Serial Module
* @param ewmaAlpha the alpha for exponentially weighted moving average
* smoothing. By default, no smoothing happens (alpha=1)
*/
ACEINNAInclinometer(HardwareSerial &canSerialInterface)
: canInterface(canSerialInterface)
ACEINNAInclinometer(HardwareSerial &canSerialInterface,
float ewmaAlpha = 1.0)
: canInterface(canSerialInterface), hasDataCached(false),
roll(0, ewmaAlpha), pitch(0, ewmaAlpha)
{
}

@@ -42,20 +47,25 @@ class ACEINNAInclinometer : public InclinometerDataSource {
//! Address to simulate with this controller
static constexpr byte k_sourceAddress = 0x11;

/**
* @brief PGNs used by this module
*/
enum PGN {
// Data Messages / Get Messages
PGN_ENABLED_PERIODIC_DATA_TYPES = 61366,
PGN_SSI2DATA = 61481,

// Command Messages
PGN_ODR = 65365,
PGN_PERIODIC_DATA_TYPES,
PGN_LOW_PASS,
PGN_ORIENTATION
};
//! Allowed angle range before considering the inclinometer data to be
//! invalid (degrees)
static constexpr double k_anglePlausibilityRange = 5;

/**
* @brief PGNs used by this module
*/
enum PGN {
// Data Messages / Get Messages
PGN_ENABLED_PERIODIC_DATA_TYPES = 61366,
PGN_SSI2DATA = 61481,

// Command Messages
PGN_SAVE_EEPROM = 65361,
PGN_ODR = 65365,
PGN_PERIODIC_DATA_TYPES,
PGN_LOW_PASS,
PGN_ORIENTATION
};

//! See the InclinometerDataSource interface

@@ -65,6 +75,12 @@ class ACEINNAInclinometer : public InclinometerDataSource {

private:
CAN::J1939Interface canInterface;

MovingAverage roll;
MovingAverage pitch;

Eigen::Vector2d cachedAngles;
bool hasDataCached;
};
}; // namespace Inclinometer

4 changes: 0 additions & 4 deletions ADXL355Inclinometer.cpp
Original file line number Diff line number Diff line change
@@ -15,10 +15,6 @@ Eigen::Vector2d Inclinometer::ADXL355Inclinometer::getData()
Fault::Handler::instance()->setFaultCode(
Fault::INCLINOMETER_IMPLAUSIBLE_READING);
}
else {
Fault::Handler::instance()->unlatchFaultCode(
Fault::INCLINOMETER_IMPLAUSIBLE_READING);
}

// Apply EWMA filtering
movingAverageFilter.addData(measure);
32 changes: 1 addition & 31 deletions CANInterface.cpp
Original file line number Diff line number Diff line change
@@ -31,41 +31,11 @@ void CAN::RawInterface::resetBaudTo(CAN::SerialBaudrate s)
sprintf(atCommand, "AT+S=%d", (unsigned int)s);
atCommand[6] = '\0';
serialInterface.println(atCommand);
delay(100);
Serial.println("did one");
delay(50);
flushBuffer();
}
serialInterface.begin(serialBaudToBaud(s));
serialInterface.print("+++");
/*char atCommand[7];
sprintf(atCommand, "AT+S=%d", (unsigned int)s);
atCommand[6] = '\0';
serialInterface.begin(9600);
serialInterface.print("+++");
serialInterface.println(atCommand);
delay(100);
serialInterface.begin(19200);
serialInterface.print("+++");
serialInterface.println(atCommand);
delay(100);
serialInterface.begin(38400);
serialInterface.print("+++");
serialInterface.println(atCommand);
delay(100);
serialInterface.begin(57600);
serialInterface.print("+++");
serialInterface.println(atCommand);
delay(100);
serialInterface.begin(115200);
serialInterface.print("+++");
serialInterface.println(atCommand);
delay(100);
serialInterface.begin(serialBaudToBaud(s));
serialInterface.print("+++");*/
flushBuffer();
}

29 changes: 29 additions & 0 deletions CANSAEJ1939.h
Original file line number Diff line number Diff line change
@@ -105,6 +105,9 @@ struct J1939Message {
* @brief Construct a new J1939Message object representing a command message
* with one byte of data in the payload
*
* The first byte of this message is actually the desination address. The
* following byte is data 1
*
* @param source source CAN address of the message
* @param dest destination CAN address of the message
* @param PGN parameter group number of the message
@@ -122,6 +125,9 @@ struct J1939Message {
* @brief Construct a new J1939Message object representing a command message
* with two bytes of data in the payload
*
* NOTE: The first byte of this message is actually the desination address.
* The following byte is data 1
*
* @param source source CAN address of the message
* @param dest destination CAN address of the message
* @param PGN parameter group number of the message
@@ -139,6 +145,29 @@ struct J1939Message {
CanID = J1939ID(6, source, PGN);
}

/**
* @brief Construct a new J1939Message object representing a command message
* with three bytes of data in the payload
*
* @param source source CAN address of the message
* @param dest destination CAN address of the message
* @param PGN parameter group number of the message
* @param data1 the first byte of data (leftmost byte) to send, filling the
* rest of the bytes with zeros
* @param data2 the second byte of data (second-to-leftmost byte) to send,
* filling the rest of the bytes with zeros
* @param data3 the second byte of data (third-to-leftmost byte) to send,
* filling the rest of the bytes with zeros
*/
J1939Message(byte source, byte dest, unsigned long PGN, byte data1,
byte data2, byte data3)
{
data[0] = data1;
data[1] = data2;
data[2] = data3;
CanID = J1939ID(6, source, PGN);
}

/**
* @brief Construct a new J1939Message object by parsing a CAN2.0B extended
* packet
10 changes: 10 additions & 0 deletions FaultHandling.cpp
Original file line number Diff line number Diff line change
@@ -32,6 +32,16 @@ void Fault::Handler::unlatchFaultCode(Type fault)
this->faults[fault] = false;
}

void Fault::Handler::onFaultUnlatchEvent(FaultUnlatchEvent event)
{
bool* evtTriggers = k_faultUnlatchMapping[event];
for (int i = 0; i < RETRYABLE_END_SENTINEL - FATAL_END_SENTINEL; ++i) {
if (evtTriggers[i]) {
unlatchFaultCode(FATAL_END_SENTINEL + 1 + i);
}
}
}

bool Fault::Handler::hasFault(Type fault) { return this->faults[fault]; }

bool Fault::Handler::hasFaultOfType(Type start, Type end)
37 changes: 31 additions & 6 deletions FaultHandling.h
Original file line number Diff line number Diff line change
@@ -31,6 +31,23 @@ enum Type {
ALL_OK
};

/**
* @brief List of system events that can unlatch one or more recoverable faults
*/
enum FaultUnlatchEvent {
INCLINOMETER_DATA_RECEIVE = 0,
MOVEMENT_COMMAND_END,
FAULT_UNLATCH_EVENT_SIZE
};

// clang-format off
constexpr bool k_faultUnlatchMapping[FAULT_UNLATCH_EVENT_SIZE][RETRYABLE_END_SENTINEL - FATAL_END_SENTINEL] = {
// INCL_NR IMPLAUS PARITY
{true, false, false}, // INCLINOMETER_DATA_RECEIVE
{false, true, false} // MOVEMENT_COMMAND_END
};
// clang-format on

/**
* @brief Fault handler singleton
*/
@@ -59,7 +76,8 @@ class Handler {
bool hasMinorFault() { return hasFaultOfType(FATAL_END_SENTINEL, ALL_OK); };

/**
* @brief Checks if there is at least one major (non-recoverable) fault latched
* @brief Checks if there is at least one major (non-recoverable) fault
* latched
*
* @return true if there is at least one major fault
* @return false if there aren't any major faults
@@ -76,23 +94,30 @@ class Handler {

/**
* @brief periodic function that writes fault flash codes to an indicator.
*
*
* Note - will only flash the first fault found.
*
*
* 2 short flashes plus n long flashes for recoverable faults
* 5 short flashes plus n long flashes for non-recoverable faults
*
*
* @param LEDPIN pin to digital write error code patterns to
*/
void faultFlasherPeriodic(const int LEDPIN);

/**
* @brief unlatch a fault condition
*
* @brief unlatch a fault condition directly
*
* @param fault the fault condition to unlatch
*/
void unlatchFaultCode(Type fault);

/**
* @brief Unlatches all faults that are mapped to this fault unlatch event.
*
* @param event the event that was triggered
*/
void onFaultUnlatchEvent(FaultUnlatchEvent event);

private:
bool faults[ALL_OK];
static Handler *inst;
13 changes: 12 additions & 1 deletion InclinometerModel.cpp
Original file line number Diff line number Diff line change
@@ -45,7 +45,18 @@ Vector2d Inclinometer::Model::calculate(Vector2d angleMeasures)
Matrix3d measuredFrame = convertAnglesToFrame(angleMeasures);
Vector3d angles = (this->baseFrame * zeroFrame.transpose() * measuredFrame)
.eulerAngles(0, 1, 2);
return Vector2d(angles[1], angles[0]);

Vector2d calculated = Vector2d(angles[1], angles[0]);

rollVelocity.addPoint(calculated[0] - lastAngles[0]);
pitchVelocity.addPoint(calculated[1] - lastAngles[1]);
lastAngles = calculated;
return calculated;
}

Vector2d Inclinometer::Model::getAngularAveragedVelocities()
{
return Vector2d(rollVelocity.getAverage(), pitchVelocity.getAverage());
}

Matrix3d Inclinometer::Model::convertAnglesToFrame(Vector2d angleMeasures)
Loading
Oops, something went wrong.

0 comments on commit c76eb34

Please sign in to comment.