Merge branch 'master' into navigation_testing

Author: lwaghorn

OnPod/FlightComputer/include/CanManager.h

@@ -47,5 +47,14 @@ T extractCanValue(const __u8 data[], const std::vector<int> &byteIndices, T conv
     return tConverted;
 }
 
+class BroadcastManager{
+public:
+    BroadcastManager();
+    ~BroadcastManager();
+    void addBroadcast(int);
+private:
+    std::vector<int> _broadcastSockts = {};
+
+};
 
 #endif //FLIGHTCOMPUTER_CANMANAGER_H

OnPod/FlightComputer/include/Structs.h

@@ -28,6 +28,7 @@ struct PodValues {
     uint32_t accelerationTime = 0;
     uint32_t expectedTubePressure = 0;
     uint32_t maxVelocity = 0;
+    uint32_t maxRPM = 0;
     uint32_t brakeDistance = 0;
     uint32_t maxStripCount = 0;
     bool taxi  = false;

OnPod/FlightComputer/src/Communication/CanManager.cpp

@@ -2,6 +2,19 @@
 #include <algorithm>
 #include <initializer_list>
 
+
+BroadcastManager::BroadcastManager() = default;
+
+void BroadcastManager::addBroadcast(int socket) {
+    this->_broadcastSockts.push_back(socket);
+}
+
+BroadcastManager::~BroadcastManager() {
+    for(auto socket : _broadcastSockts){
+        close(socket);
+    }
+}
+
 void processFrame(const struct can_frame &frame, TelemetryManager &pod) {
     std::vector<int> indices = {0, 1};
     switch (frame.can_id) {

OnPod/FlightComputer/src/Constants/Constants.h

@@ -14,8 +14,10 @@ namespace GENERAL_CONSTANTS
 
     static const float   REAR_WHEEL_CIRCUMFRENCE = 0.933619f;
     static const float   STRIP_DISTANCE = 30.48;
+    static const float   DEFAULT_MAX_STRIP_COUNT = 80;
 
-    static const uint32_t NAV_SERIAL_MESSAGE_SIZE = 7;
+    static const int32_t NAV_SERIAL_MESSAGE_SIZE = 10;
+    static const float   STATIONARY_THRSHOLD_M = 0.5f;
 };
 
 namespace CONNECTION_FLAGS

OnPod/FlightComputer/src/States.cpp

@@ -55,13 +55,13 @@ bool PodState::isNodeSensorCritical(uint32_t sensorIndex) {
 }
 
 bool PodState::isConnectionFlagCritical(uint32_t sensorIndex) {
-    uint32_t criticalMask = CONNECTION_FLAGS::BRAKE_NODE_HEARTBEAT_INDEX |
-                            CONNECTION_FLAGS::LVDC_NODE_HEARTBEAT_INDEX |
-                            CONNECTION_FLAGS::BMS_HEARTBEAT_INDEX |
-                            CONNECTION_FLAGS::INTERFACE_HEARTBEAT_INDEX |
-                            CONNECTION_FLAGS::ENCLOSURE_HEARTBEAT_INDEX |
-                            CONNECTION_FLAGS::NAVIGATION_HEARTBEAT_INDEX;
-    return sensorIndex & criticalMask;
+    std::vector<int> criticalSensors = {CONNECTION_FLAGS::BRAKE_NODE_HEARTBEAT_INDEX,
+                                        CONNECTION_FLAGS::LVDC_NODE_HEARTBEAT_INDEX,
+                                        CONNECTION_FLAGS::BMS_HEARTBEAT_INDEX,
+                                        CONNECTION_FLAGS::INTERFACE_HEARTBEAT_INDEX,
+                                        CONNECTION_FLAGS::ENCLOSURE_HEARTBEAT_INDEX,
+                                        CONNECTION_FLAGS::NAVIGATION_HEARTBEAT_INDEX};
+    return std::find(criticalSensors.begin(), criticalSensors.end(), sensorIndex) != criticalSensors.end();
 }
 
 bool PodState::isInverterSensorCritical(uint32_t sensorIndex) {
@@ -148,7 +148,6 @@ void PodState::armedChecks(){
         std::string error = "Failed on inverter fault, see pod messages";
         throw std::runtime_error(error);
     }
-
 }
 
 bool PodState::brakingCriteriaMet() {
@@ -163,6 +162,7 @@ bool PodState::brakingCriteriaMet() {
         pod->sendUpdate("Braking at maximum strip count");
         return true;
     }
+
     return false;
 }
 
@@ -295,6 +295,7 @@ Arming::Arming(TelemetryManager * pod ): PodState(pod) {
     _stateIdentifier = psArming;
     this->pod->telemetry->commandedBrakeNodeState = bnsStandby;
     this->pod->telemetry->commandedLvdcNodeState = lvdcFlight;
+    this->pod->telemetry->podPosition = 0.0f;
 }
 
 Arming::~Arming() = default;
@@ -439,6 +440,10 @@ bool Acceleration::testTransitions() {
         this->setupTransition(psBraking, (std::string)" Flight Timout of " + std::to_string(this->timeInStateSeconds()) + " reached. Pod --> Braking");
         return true;
     }
+
+    if(this->pod->telemetry->motorSpeed >= pod->telemetry->maxRPM){
+        this->setupTransition(psCoasting, "Acceleration->Coasting from maximum speed");
+    }
     return false;
 }
 
@@ -449,7 +454,8 @@ Coasting::Coasting(TelemetryManager* pod) : PodState(pod) {
     this->pod->telemetry->commandedTorque = 0;
 }
 
-bool Coasting::testTransitions() {
+bool Coasting::testTransitions() {   
+    this->pod->telemetry->commandedTorque = 0;
     if(this->pod->getControlsInterfaceState() == ciEmergencyStop){
         this->setupTransition(psBraking, "Emergency Stop. Pod --> Braking");
         return true;

OnPod/FlightComputer/src/TelemetryManager.cpp

@@ -361,12 +361,12 @@ void TelemetryManager::setMotorSpeed(int32_t value) {
     auto thisTime = std::chrono::high_resolution_clock::now();
     this->telemetry->lastMotorReadTime = thisTime;
     int32_t lastRPM = this->telemetry->motorSpeed;
-    this->telemetry->motorSpeed = value;
+    this->telemetry->motorSpeed = value; 
     float average = (value+lastRPM)/2.0;
     average /= (1000.0*60.0);
+
     float milliseconds = (std::chrono::duration_cast<std::chrono::microseconds>(thisTime - lastTime).count())/1000.0;
     float distance = average*milliseconds * GENERAL_CONSTANTS::REAR_WHEEL_CIRCUMFRENCE;
-
     addPodDistance(distance);
 }
 

OnPod/FlightComputer/src/Threads/CanBusThread.cpp

@@ -159,11 +159,11 @@ void startInverterBroadcast(int bcmSocket){
     }
 }
 
-void setInverterTorque(int torque, int bcmSocket){
+void setInverterTorque(int32_t torque, int bcmSocket){
     struct broadcastManagerConfig msg = {};
     torque *=10;
-    int32_t highByte = torque/256;
-    int32_t lowByte = torque%256;
+    uint16_t highByte = torque/256;
+    uint16_t lowByte = torque%256;
     msg.msg_head.opcode  = TX_SETUP;
     msg.msg_head.can_id  = 0;
     msg.msg_head.flags   = SETTIMER | STARTTIMER;
@@ -239,6 +239,7 @@ int canNetworkThread(TelemetryManager Pod){
     LOG(INFO) << "Starting CAN Thread on ";
     int32_t canSockRaw = 0;
     int32_t canSockBcm = 0;
+    BroadcastManager manager = BroadcastManager();
     try{
         canSockRaw = getCanSocketRaw();
     }
@@ -248,13 +249,14 @@ int canNetworkThread(TelemetryManager Pod){
     }
     try{
         canSockBcm = getCanSocketBrodcastManager();
+        manager.addBroadcast(canSockBcm);
     }
     catch (std::runtime_error &e){
         LOG(INFO) << e.what();
         return -1;
     }
     try{
-       //  startInverterBroadcast(canSockBcm);
+       ;//  startInverterBroadcast(canSockBcm);
     }
     catch (const std::runtime_error &error){
         LOG(INFO) << error.what();

OnPod/FlightComputer/src/Threads/CommanderThread.cpp

@@ -96,14 +96,14 @@ void parseProtoCommand(PodCommand podCommand, TelemetryManager *Pod) {
         Pod->telemetry->taxi = podCommand.taxi();
         Pod->telemetry->expectedTubePressure = podCommand.expectedtubepressure();
         Pod->telemetry->maxVelocity = podCommand.maxvelocity();
+        Pod->telemetry->maxRPM = podCommand.maxvelocity() / GENERAL_CONSTANTS::REAR_WHEEL_CIRCUMFRENCE;
         Pod->telemetry->brakeDistance = podCommand.brakingdistance();
         if(!podCommand.taxi()) {
-            float maxStripCount = (Pod->telemetry->flightDistance - Pod->telemetry->brakeDistance) /
-                                  GENERAL_CONSTANTS::STRIP_DISTANCE;
-            Pod->telemetry->maxStripCount = int32_t(ceil(maxStripCount));
+            Pod->telemetry->maxStripCount = ( (Pod->telemetry->flightDistance - Pod->telemetry->brakeDistance) /
+                                  GENERAL_CONSTANTS::STRIP_DISTANCE ) + 1;
         }
         else{
-            Pod->telemetry->maxStripCount = 999;
+            Pod->telemetry->maxStripCount = GENERAL_CONSTANTS::DEFAULT_MAX_STRIP_COUNT; 
         }
     }
     if (podCommand.has_manualbrakenodestate()){