Skip to content

Commit be93c6b

Browse files
MatinQurbanMatinQurban
authored
Merge pull request #29 from nicholas-tangerine/can_fix
CAN fix for VDM
2 parents 66ebc55 + 353ccce commit be93c6b

File tree

7 files changed

+582
-450
lines changed

7 files changed

+582
-450
lines changed

‎CANbus.dbc‎

Lines changed: 403 additions & 403 deletions
Large diffs are not rendered by default.

‎Telemetry-Main/CANProtocol.hpp‎

Lines changed: 18 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -128,14 +128,14 @@ struct PDB_POWER_B_t {
128128
};
129129

130130
struct SME_THROTTLE_DEMAND_t {
131-
uint16_t TORQUE_DEMAND; // bits 0-15
132-
uint16_t MAX_SPEED; // bits 16-31
133-
uint8_t FORWARD : 1; // bit 32
134-
uint8_t REVERSE : 1; // bit 33
135-
uint8_t padding_34 : 1; // bit 34 (used for alignment)
136-
uint8_t POWER_READY : 1; // bit 35
137-
uint8_t padding2 : 4; // bits 36-39
138-
uint8_t MBB_ALIVE: 4; // bits 40-43
131+
uint16_t TORQUE_DEMAND; // bits 0-15
132+
uint16_t MAX_SPEED; // bits 16-31
133+
uint8_t FORWARD : 1; // bit 32
134+
uint8_t REVERSE : 1; // bit 33
135+
uint8_t padding_34 : 1; // bit 34 (used for alignment)
136+
uint8_t POWER_READY : 1; // bit 35
137+
uint8_t padding2 : 4; // bits 36-39
138+
uint8_t MBB_ALIVE: 4; // bits 40-43
139139
};
140140

141141
struct SME_TRQSPD_t {
@@ -199,13 +199,13 @@ struct TPERIPH_TIRETEMP_t {
199199
};
200200

201201
struct VDM_GPS_LAT_LONG_t {
202-
int32_t LATITUDE; // bits 7-38
203-
int32_t LONGITUDE; // bits 39-50
202+
float LATITUDE; // bits 7-38
203+
float LONGITUDE; // bits 39-50
204204
};
205205

206206
struct VDM_GPS_DATA_t {
207207
uint16_t SPEED; // bits 7-22
208-
int16_t ALTITUDE; // bits 23-38
208+
int16_t ALTITUDE; // bits 23-38
209209
uint16_t TRUE_COURSE; // bits 39-54
210210
uint8_t SATELLITES_IN_USE; // bits 55-62
211211
uint8_t VALID1; // bits 63-70
@@ -216,21 +216,22 @@ struct VDM_DATE_TIME_t {
216216
uint8_t UTC_DATE_YEAR; // bits 15-22
217217
uint8_t UTC_DATE_MONTH; // bits 23-30
218218
uint8_t UTC_DATE_DAY; // bits 31-38
219+
uint8_t padding; // bits 39-46
219220
uint8_t UTC_TIME_HOURS; // bits 47-54
220221
uint8_t UTC_TIME_MINUTES; // bits 55-62
221222
uint8_t UTC_TIME_SECONDS; // bits 63-70
222223
};
223224

224225
struct VDM_ACCELERATION_t {
225-
int16_t X; // bits 7-22
226-
int16_t Y; // bits 23-38
227-
int16_t Z; // bits 39-54
226+
int16_t X; // bits 7-22
227+
int16_t Y; // bits 23-38
228+
int16_t Z; // bits 39-54
228229
};
229230

230231
struct VDM_YAW_RATE_t {
231-
int16_t X; // bits 7-22
232-
int16_t Y; // bits 23-38
233-
int16_t Z; // bits 39-54
232+
int16_t X; // bits 7-22
233+
int16_t Y; // bits 23-38
234+
int16_t Z; // bits 39-54
234235
};
235236

236237
#endif // CANPROTOCOL_HPP

‎Telemetry-Main/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,6 @@ add_library(fsdaq STATIC ./fsdaq/data_logger.cpp) # ./fsdaq/can_processor_genera
2323
target_include_directories(fsdaq PUBLIC ./fsdaq)
2424
target_link_libraries(fsdaq mbed-core-flags mbed-storage-sd mbed-storage-fat libradio)
2525

26-
add_executable(${PROJECT_NAME} main.cpp VehicleStateManager.cpp)
26+
add_executable(${PROJECT_NAME} main.cpp VehicleStateManager.cpp LapCounter.cpp)
2727
target_link_libraries(${PROJECT_NAME} mbed-os dash fsdaq libradio mbed-storage-sd mbed-storage-fat)
2828
mbed_set_post_build(${PROJECT_NAME})

‎Telemetry-Main/LapCounter.cpp‎

Lines changed: 94 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,94 @@
1+
#include <cmath>
2+
3+
#include "LapCounter.hpp"
4+
5+
#define DEG_TO_RAD ((float) M_PI / 180.0f)
6+
7+
#define EARTH_RADIUS 6378137 // earths radius in meters, used for conversion to local coordinate plane
8+
#define X_TOLERANCE 8.0f // check lap completion if x is within +- X_TOLERANCE meters of home x coord
9+
#define Y_TOLERANCE 8.0f // check lap completion if y is within +- Y_TOLERANCE meters of home y coord
10+
#define HEADING_TOLERANCE 80.0f // check lap completion if heading is within +- HEADING_TOLERANCE degrees of home
11+
#define MIN_LAP_TIME 10 // minimum lap time in seconds. used to ensure same lap isn't counted twice
12+
13+
using namespace std;
14+
15+
LapCounter::LapCounter(VehicleState state) {
16+
resetLapCounter(state);
17+
}
18+
19+
void LapCounter::resetLapCounter(VehicleState state) {
20+
data.lap_home_latitude_f = state.vdmGpsLatLong.LATITUDE;
21+
data.lap_home_longitude_f = state.vdmGpsLatLong.LONGITUDE;
22+
data.lap_home_x_f = 0.0f;
23+
data.lap_home_y_f = 0.0f;
24+
data.lap_home_heading_f = (float) (state.vdmGpsData.TRUE_COURSE % 36000) * 0.01f; //scale to 0-359.99
25+
26+
data.lap_prev_latitude_f = 0.0f;
27+
data.lap_prev_longitude_f = 0.0f;
28+
data.lap_prev_x_f = 0.0f;
29+
data.lap_prev_y_f = 0.0f;
30+
data.lap_prev_heading_f = 0.0f;
31+
32+
data.lap_curr_latitude_f = data.lap_home_latitude_f;
33+
data.lap_curr_longitude_f = data.lap_home_longitude_f;
34+
data.lap_curr_x_f = data.lap_home_x_f;
35+
data.lap_curr_y_f = data.lap_home_y_f;
36+
data.lap_curr_heading_f = data.lap_home_heading_f;
37+
38+
data.lap_counter = 0;
39+
40+
timer.reset();
41+
timer.start();
42+
}
43+
44+
void LapCounter::updateLapCounter(VehicleState state) {
45+
/**
46+
* UPDATE POSITION
47+
*/
48+
// Get prev data, weighted avg to average out noise
49+
data.lap_prev_latitude_f = 0.1f * data.lap_prev_latitude_f + 0.9f * data.lap_curr_latitude_f;
50+
data.lap_prev_longitude_f = 0.1f * data.lap_prev_longitude_f + 0.9f * data.lap_curr_longitude_f;
51+
data.lap_prev_x_f = 0.1f * data.lap_prev_x_f + 0.9f * data.lap_curr_x_f;
52+
data.lap_prev_y_f = 0.1f * data.lap_prev_y_f + 0.9f * data.lap_curr_y_f;
53+
data.lap_prev_heading_f = 0.1f * data.lap_prev_heading_f + 0.9f * data.lap_curr_heading_f;
54+
55+
// Get curr data, transform to local coordinate plane
56+
data.lap_curr_latitude_f = state.vdmGpsLatLong.LATITUDE;
57+
data.lap_curr_longitude_f = state.vdmGpsLatLong.LONGITUDE;
58+
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
59+
data.lap_curr_y_f = EARTH_RADIUS * DEG_TO_RAD * (data.lap_curr_latitude_f - data.lap_home_latitude_f);
60+
data.lap_curr_heading_f = (float) (state.vdmGpsData.TRUE_COURSE % 36000) * 0.01f; //scale to 0-359.99
61+
62+
// TRANSFORM: Apply rotation matrix, home heading is x axis.
63+
float home_heading_rad_f = DEG_TO_RAD * data.lap_home_heading_f; // convert to rad for trig funcs
64+
float temp_x_f = data.lap_curr_x_f * cosf(home_heading_rad_f) + data.lap_curr_y_f * sinf(home_heading_rad_f);
65+
float temp_y_f = data.lap_curr_x_f * -sinf(home_heading_rad_f) + data.lap_curr_y_f * cosf(home_heading_rad_f);
66+
data.lap_curr_x_f = temp_x_f;
67+
data.lap_curr_y_f = temp_y_f;
68+
69+
/**
70+
* CHECK IF LAP COMPLETED
71+
*/
72+
// If nowhere near home coords/heading, return immediately
73+
if (abs(data.lap_curr_x_f - data.lap_home_x_f) > X_TOLERANCE) return;
74+
if (abs(data.lap_curr_y_f - data.lap_home_y_f) > Y_TOLERANCE) return;
75+
if (abs(data.lap_curr_heading_f - data.lap_home_heading_f) > HEADING_TOLERANCE) return;
76+
if (timer.elapsed_time().count() < MIN_LAP_TIME) {
77+
timer.reset();
78+
timer.start();
79+
80+
return;
81+
}
82+
83+
// Lap has been completed, all conditions pass
84+
data.lap_counter++;
85+
data.lap_time = std::chrono::duration<float>{timer.elapsed_time()}.count();
86+
87+
timer.reset();
88+
timer.start();
89+
return;
90+
}
91+
92+
float LapCounter::getTime() {
93+
return std::chrono::duration<float>{timer.elapsed_time()}.count();
94+
}

‎Telemetry-Main/LapCounter.hpp‎

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
/**
2+
* Lap Counter
3+
* Author: Nicholas Tang
4+
*
5+
* Made to separate lap counting from VSM. Uses VSM GPS and HEADING/TRUE_COURSE
6+
* data. Make sure to call updateLapCounter a lot.
7+
*
8+
* Note: "local coordinate plane" is defined as: nose towards positive x, positive y is left i think
9+
*/
10+
11+
#ifndef LAP_COUNTER_HPP
12+
#define LAP_COUNTER_HPP
13+
14+
#include "CANProtocol.hpp"
15+
#include "VehicleStateManager.hpp"
16+
#include "mbed.h"
17+
18+
struct LapCounterData {
19+
float lap_home_latitude_f;
20+
float lap_home_longitude_f;
21+
float lap_home_x_f; // relative to local coordinate plane
22+
float lap_home_y_f; // relative to local coordinate plane
23+
float lap_home_heading_f;
24+
25+
float lap_prev_latitude_f;
26+
float lap_prev_longitude_f;
27+
float lap_prev_x_f; // relative to local coordinate plane
28+
float lap_prev_y_f; // relative to local coordinate plane
29+
float lap_prev_heading_f;
30+
31+
float lap_curr_latitude_f;
32+
float lap_curr_longitude_f;
33+
float lap_curr_x_f; // relative to local coordinate plane
34+
float lap_curr_y_f; // relative to local coordinate plane
35+
float lap_curr_heading_f;
36+
37+
uint8_t lap_counter; // MARK: surely we don't do more than 255 laps(?)
38+
float lap_time;
39+
};
40+
41+
class LapCounter {
42+
public:
43+
LapCounterData data;
44+
Timer timer;
45+
46+
/**
47+
* Constructor, initializes all fields to 0 but lat/long
48+
*/
49+
LapCounter(VehicleState state);
50+
/**
51+
* Resets object field. GPS data is set to wherever is in vehicle state
52+
*/
53+
void resetLapCounter(VehicleState state);
54+
/**
55+
* Checks if a lap has been completed. If so, increments lap counter
56+
*/
57+
void updateLapCounter(VehicleState state);
58+
/**
59+
* Gets the lap time in seconds.
60+
*/
61+
float getTime();
62+
};
63+
64+
#endif

‎Telemetry-Main/VehicleStateManager.cpp‎

Lines changed: 2 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
#include "VehicleStateManager.hpp"
22
#include <cstdio>
3+
#include <cstdlib>
4+
#include <cmath>
35
#include <algorithm>
46
#include <chrono>
57
#include <cstring>
@@ -16,14 +18,11 @@ VehicleStateManager::VehicleStateManager(
1618
) : _mbedCAN(mbedCAN), _steering_sensor(steering_sensor), _brake_sensor_f(brake_sensor_r), _brake_sensor_r(brake_sensor_r)
1719
{
1820
_vehicleState = {};
19-
20-
strcpy(_lapTime, "0:00.000");
2121
}
2222

2323
void VehicleStateManager::update() {
2424
// printf("Updating CAN\n");
2525
processCANMessage();
26-
updateLapTime();
2726
readSensorValues();
2827
}
2928

@@ -198,25 +197,6 @@ void VehicleStateManager::processCANMessage() {
198197
}
199198
}
200199

201-
void VehicleStateManager::updateLapTime() {
202-
uint32_t milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(_lapTimer.elapsed_time()).count();
203-
uint32_t minutes = milliseconds / 60000;
204-
uint32_t seconds = (milliseconds % 60000) / 1000;
205-
uint32_t ms = milliseconds % 1000;
206-
207-
snprintf(_lapTime, sizeof(_lapTime), "%lu:%02lu.%03lu",
208-
(unsigned long)minutes, (unsigned long)seconds, (unsigned long)ms);
209-
}
210-
211-
void VehicleStateManager::startLapTimer() {
212-
_lapTimer.reset();
213-
_lapTimer.start();
214-
}
215-
216-
const char* VehicleStateManager::getLapTime() const {
217-
return _lapTime;
218-
}
219-
220200
VehicleState VehicleStateManager::getState() const {
221201
return _vehicleState;
222202
}

‎Telemetry-Main/VehicleStateManager.hpp‎

Lines changed: 0 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,6 @@ struct VehicleState {
4040
float steering_sensor;
4141
float brake_sensor_f;
4242
float brake_sensor_r;
43-
4443
};
4544

4645
class VehicleStateManager {
@@ -55,8 +54,6 @@ class VehicleStateManager {
5554

5655
VehicleState getState() const;
5756
void update();
58-
void startLapTimer();
59-
const char* getLapTime() const;
6057

6158
private:
6259
MbedCAN* _mbedCAN;
@@ -66,11 +63,7 @@ class VehicleStateManager {
6663

6764
VehicleState _vehicleState;
6865

69-
char _lapTime[16];
70-
Timer _lapTimer;
71-
7266
void processCANMessage();
73-
void updateLapTime();
7467
void readSensorValues();
7568
};
7669

0 commit comments

Comments
 (0)