CAN fix for VDM

Telemetry-Main/CANProtocol.hpp

@@ -128,14 +128,14 @@ struct PDB_POWER_B_t {
 };
 
 struct SME_THROTTLE_DEMAND_t {
-    uint16_t TORQUE_DEMAND;             // bits 0-15
-    uint16_t MAX_SPEED;                 // bits 16-31
-    uint8_t FORWARD : 1;                // bit 32
-    uint8_t REVERSE : 1;                // bit 33
-    uint8_t padding_34 : 1;             // bit 34 (used for alignment)
-    uint8_t POWER_READY : 1;            // bit 35
-    uint8_t padding2 : 4;               // bits 36-39
-    uint8_t MBB_ALIVE: 4;               // bits 40-43
+    uint16_t TORQUE_DEMAND;            // bits 0-15
+    uint16_t MAX_SPEED;                // bits 16-31
+    uint8_t FORWARD : 1;               // bit 32
+    uint8_t REVERSE : 1;               // bit 33
+    uint8_t padding_34 : 1;            // bit 34 (used for alignment)
+    uint8_t POWER_READY : 1;           // bit 35
+    uint8_t padding2 : 4;              // bits 36-39
+    uint8_t MBB_ALIVE: 4;              // bits 40-43
 };
 
 struct SME_TRQSPD_t {
@@ -199,13 +199,13 @@ struct TPERIPH_TIRETEMP_t {
 };
 
 struct VDM_GPS_LAT_LONG_t {
-    int32_t LATITUDE;                 // bits 7-38
-    int32_t LONGITUDE;                // bits 39-50
+    float LATITUDE;                  // bits 7-38
+    float LONGITUDE;                 // bits 39-50
 };
 
 struct VDM_GPS_DATA_t {
     uint16_t SPEED;                    // bits 7-22
-    int16_t ALTITUDE;                 // bits 23-38
+    int16_t ALTITUDE;                  // bits 23-38
     uint16_t TRUE_COURSE;              // bits 39-54
     uint8_t  SATELLITES_IN_USE;        // bits 55-62
     uint8_t  VALID1;                   // bits 63-70
@@ -216,21 +216,22 @@ struct VDM_DATE_TIME_t {
     uint8_t UTC_DATE_YEAR;             // bits 15-22
     uint8_t UTC_DATE_MONTH;            // bits 23-30
     uint8_t UTC_DATE_DAY;              // bits 31-38
+    uint8_t padding;                   // bits 39-46
     uint8_t UTC_TIME_HOURS;            // bits 47-54
     uint8_t UTC_TIME_MINUTES;          // bits 55-62
     uint8_t UTC_TIME_SECONDS;          // bits 63-70
 };
 
 struct VDM_ACCELERATION_t {
-    int16_t X;                        // bits 7-22
-    int16_t Y;                        // bits 23-38
-    int16_t Z;                        // bits 39-54
+    int16_t X;                         // bits 7-22
+    int16_t Y;                         // bits 23-38
+    int16_t Z;                         // bits 39-54
 };
 
 struct VDM_YAW_RATE_t {
-    int16_t X;                        // bits 7-22
-    int16_t Y;                        // bits 23-38
-    int16_t Z;                        // bits 39-54
+    int16_t X;                         // bits 7-22
+    int16_t Y;                         // bits 23-38
+    int16_t Z;                         // bits 39-54
 };
 
 #endif // CANPROTOCOL_HPP

Telemetry-Main/CMakeLists.txt

@@ -23,6 +23,6 @@ add_library(fsdaq STATIC ./fsdaq/data_logger.cpp) # ./fsdaq/can_processor_genera
 target_include_directories(fsdaq PUBLIC ./fsdaq)
 target_link_libraries(fsdaq mbed-core-flags mbed-storage-sd mbed-storage-fat libradio)
 
-add_executable(${PROJECT_NAME} main.cpp VehicleStateManager.cpp)
+add_executable(${PROJECT_NAME} main.cpp VehicleStateManager.cpp LapCounter.cpp)
 target_link_libraries(${PROJECT_NAME} mbed-os dash fsdaq libradio mbed-storage-sd mbed-storage-fat)
 mbed_set_post_build(${PROJECT_NAME})

Telemetry-Main/LapCounter.cpp

@@ -0,0 +1,94 @@
+#include <cmath>
+
+#include "LapCounter.hpp"
+
+#define DEG_TO_RAD ((float) M_PI / 180.0f)
+
+#define EARTH_RADIUS 6378137    // earths radius in meters, used for conversion to local coordinate plane
+#define X_TOLERANCE 8.0f        // check lap completion if x is within +- X_TOLERANCE meters of home x coord
+#define Y_TOLERANCE 8.0f        // check lap completion if y is within +- Y_TOLERANCE meters of home y coord
+#define HEADING_TOLERANCE 80.0f // check lap completion if heading is within +- HEADING_TOLERANCE degrees of home
+#define MIN_LAP_TIME 10         // minimum lap time in seconds. used to ensure same lap isn't counted twice
+
+using namespace std;
+
+LapCounter::LapCounter(VehicleState state) {
+    resetLapCounter(state);
+}
+
+void LapCounter::resetLapCounter(VehicleState state) {
+    data.lap_home_latitude_f = state.vdmGpsLatLong.LATITUDE;
+    data.lap_home_longitude_f = state.vdmGpsLatLong.LONGITUDE;
+    data.lap_home_x_f = 0.0f;
+    data.lap_home_y_f = 0.0f;
+    data.lap_home_heading_f = (float) (state.vdmGpsData.TRUE_COURSE % 36000) * 0.01f; //scale to 0-359.99
+
+    data.lap_prev_latitude_f = 0.0f;
+    data.lap_prev_longitude_f = 0.0f;
+    data.lap_prev_x_f = 0.0f;
+    data.lap_prev_y_f = 0.0f;
+    data.lap_prev_heading_f = 0.0f;
+
+    data.lap_curr_latitude_f = data.lap_home_latitude_f;
+    data.lap_curr_longitude_f = data.lap_home_longitude_f;
+    data.lap_curr_x_f = data.lap_home_x_f;
+    data.lap_curr_y_f = data.lap_home_y_f;
+    data.lap_curr_heading_f = data.lap_home_heading_f;
+
+    data.lap_counter = 0;
+
+    timer.reset();
+    timer.start();
+}
+
+void LapCounter::updateLapCounter(VehicleState state) {
+    /**
+     * UPDATE POSITION
+     */
+    //  Get prev data, weighted avg to average out noise 
+    data.lap_prev_latitude_f = 0.1f * data.lap_prev_latitude_f + 0.9f * data.lap_curr_latitude_f;
+    data.lap_prev_longitude_f = 0.1f * data.lap_prev_longitude_f + 0.9f * data.lap_curr_longitude_f;
+    data.lap_prev_x_f = 0.1f * data.lap_prev_x_f + 0.9f * data.lap_curr_x_f;
+    data.lap_prev_y_f = 0.1f * data.lap_prev_y_f + 0.9f * data.lap_curr_y_f;
+    data.lap_prev_heading_f = 0.1f * data.lap_prev_heading_f + 0.9f * data.lap_curr_heading_f;
+
+    //  Get curr data, transform to local coordinate plane
+    data.lap_curr_latitude_f = state.vdmGpsLatLong.LATITUDE;
+    data.lap_curr_longitude_f = state.vdmGpsLatLong.LONGITUDE;
+    data.lap_curr_x_f = EARTH_RADIUS * DEG_TO_RAD * (data.lap_curr_longitude_f - data.lap_home_longitude_f) * cosf(DEG_TO_RAD * data.lap_home_latitude_f);  // lat, long is degrees -> translate to rad
+    data.lap_curr_y_f = EARTH_RADIUS * DEG_TO_RAD * (data.lap_curr_latitude_f - data.lap_home_latitude_f);
+    data.lap_curr_heading_f = (float) (state.vdmGpsData.TRUE_COURSE % 36000) * 0.01f; //scale to 0-359.99
+
+    //  TRANSFORM: Apply rotation matrix, home heading is x axis.
+    float home_heading_rad_f = DEG_TO_RAD * data.lap_home_heading_f;           // convert to rad for trig funcs
+    float temp_x_f = data.lap_curr_x_f * cosf(home_heading_rad_f) + data.lap_curr_y_f * sinf(home_heading_rad_f);
+    float temp_y_f = data.lap_curr_x_f * -sinf(home_heading_rad_f) + data.lap_curr_y_f * cosf(home_heading_rad_f);
+    data.lap_curr_x_f = temp_x_f;
+    data.lap_curr_y_f = temp_y_f;
+
+    /**
+     * CHECK IF LAP COMPLETED
+     */
+    //  If nowhere near home coords/heading, return immediately
+    if (abs(data.lap_curr_x_f - data.lap_home_x_f) > X_TOLERANCE) return;
+    if (abs(data.lap_curr_y_f - data.lap_home_y_f) > Y_TOLERANCE) return;
+    if (abs(data.lap_curr_heading_f - data.lap_home_heading_f) > HEADING_TOLERANCE) return;
+    if (timer.elapsed_time().count() < MIN_LAP_TIME) {
+        timer.reset();
+        timer.start();
+
+        return;
+    }
+
+    //  Lap has been completed, all conditions pass
+    data.lap_counter++;
+    data.lap_time = std::chrono::duration<float>{timer.elapsed_time()}.count();
+
+    timer.reset();
+    timer.start();
+    return;
+}
+
+float LapCounter::getTime() {
+    return std::chrono::duration<float>{timer.elapsed_time()}.count();
+}

Telemetry-Main/LapCounter.hpp

@@ -0,0 +1,64 @@
+/**
+ * Lap Counter
+ * Author: Nicholas Tang
+ *
+ * Made to separate lap counting from VSM. Uses VSM GPS and HEADING/TRUE_COURSE
+ * data. Make sure to call updateLapCounter a lot.
+ *
+ * Note: "local coordinate plane" is defined as: nose towards positive x, positive y is left i think
+ */
+
+#ifndef LAP_COUNTER_HPP
+#define LAP_COUNTER_HPP
+
+#include "CANProtocol.hpp"
+#include "VehicleStateManager.hpp"
+#include "mbed.h"
+
+struct LapCounterData {
+    float lap_home_latitude_f;
+    float lap_home_longitude_f;
+    float lap_home_x_f;                 // relative to local coordinate plane
+    float lap_home_y_f;                 // relative to local coordinate plane
+    float lap_home_heading_f;
+
+    float lap_prev_latitude_f;
+    float lap_prev_longitude_f;
+    float lap_prev_x_f;                 // relative to local coordinate plane
+    float lap_prev_y_f;                 // relative to local coordinate plane
+    float lap_prev_heading_f;
+
+    float lap_curr_latitude_f;
+    float lap_curr_longitude_f;
+    float lap_curr_x_f;                 // relative to local coordinate plane
+    float lap_curr_y_f;                 // relative to local coordinate plane
+    float lap_curr_heading_f;
+
+    uint8_t lap_counter;                // MARK: surely we don't do more than 255 laps(?)
+    float lap_time;
+};
+
+class LapCounter {
+    public:
+        LapCounterData data;
+        Timer timer;
+
+        /**
+         * Constructor, initializes all fields to 0 but lat/long
+         */
+        LapCounter(VehicleState state);
+        /**
+         * Resets object field. GPS data is set to wherever is in vehicle state
+         */
+        void resetLapCounter(VehicleState state);
+        /**
+         * Checks if a lap has been completed. If so, increments lap counter
+         */
+        void updateLapCounter(VehicleState state);
+        /**
+         * Gets the lap time in seconds. 
+         */
+        float getTime();
+};
+
+#endif

Telemetry-Main/VehicleStateManager.cpp

@@ -1,5 +1,7 @@
 #include "VehicleStateManager.hpp"
 #include <cstdio>
+#include <cstdlib>
+#include <cmath>
 #include <algorithm>
 #include <chrono>
 #include <cstring>
@@ -16,14 +18,11 @@ VehicleStateManager::VehicleStateManager(
 ) : _mbedCAN(mbedCAN), _steering_sensor(steering_sensor), _brake_sensor_f(brake_sensor_r), _brake_sensor_r(brake_sensor_r)
 {
     _vehicleState = {};
-
-    strcpy(_lapTime, "0:00.000");
 }
 
 void VehicleStateManager::update() {
     // printf("Updating CAN\n");
     processCANMessage();
-    updateLapTime();
     readSensorValues();
 }
 
@@ -198,25 +197,6 @@ void VehicleStateManager::processCANMessage() {
     }
 }
 
-void VehicleStateManager::updateLapTime() {
-    uint32_t milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(_lapTimer.elapsed_time()).count();
-    uint32_t minutes = milliseconds / 60000;
-    uint32_t seconds = (milliseconds % 60000) / 1000;
-    uint32_t ms = milliseconds % 1000;
-
-    snprintf(_lapTime, sizeof(_lapTime), "%lu:%02lu.%03lu", 
-             (unsigned long)minutes, (unsigned long)seconds, (unsigned long)ms);
-}
-
-void VehicleStateManager::startLapTimer() {
-    _lapTimer.reset();
-    _lapTimer.start();
-}
-
-const char* VehicleStateManager::getLapTime() const {
-    return _lapTime;
-}
-
 VehicleState VehicleStateManager::getState() const {
     return _vehicleState;
 }

Telemetry-Main/VehicleStateManager.hpp

@@ -40,7 +40,6 @@ struct VehicleState {
     float steering_sensor;
     float brake_sensor_f;
     float brake_sensor_r;
-
 };
 
 class VehicleStateManager {
@@ -55,8 +54,6 @@ class VehicleStateManager {
 
     VehicleState getState() const;
     void update();
-    void startLapTimer();
-    const char* getLapTime() const;
 
 private:
     MbedCAN* _mbedCAN;
@@ -66,11 +63,7 @@ class VehicleStateManager {
 
     VehicleState _vehicleState;
 
-    char _lapTime[16];
-    Timer _lapTimer;
-
     void processCANMessage();
-    void updateLapTime();
     void readSensorValues();
 };