Skip to content

Commit 475db66

Browse files
avtokuavtoku
committed
switch to non ubx GnssStruct
1 parent ea0c9c4 commit 475db66

File tree

9 files changed

+237
-137
lines changed

9 files changed

+237
-137
lines changed

‎boards/varmint_h7/common/Varmint.cpp‎

Lines changed: 71 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -239,6 +239,66 @@ void Varmint::battery_current_set_multiplier(double multiplier)
239239
adc_.setScaleFactor(ADC_BATTERY_CURRENT, multiplier);
240240
}
241241

242+
///////////////////////////////////////////////////////////////////////////////////////////////
243+
// GNSS
244+
//bool Varmint::gnss_read(rosflight_firmware::GnssStruct * gnss)
245+
//{
246+
// UbxPacket p;
247+
//
248+
// if (gps_.rxFifoReadMostRecent((uint8_t *) &p, sizeof(p))) {
249+
// gnss->header = p.header;
250+
// gnss->pps = p.pps;
251+
// gnss->unix_seconds = p.unix_seconds; // Unix time
252+
// gnss->t_acc = p.pvt.tAcc;
253+
// gnss->time_of_week = p.pvt.iTOW;
254+
// gnss->year = p.pvt.year;
255+
// gnss->month = p.pvt.month;
256+
// gnss->day = p.pvt.day;
257+
// gnss->hour = p.pvt.hour;
258+
// gnss->min = p.pvt.min;
259+
// gnss->sec = p.pvt.sec;
260+
// gnss->u = p.pvt.nano;
261+
// gnss->lon = p.pvt.lon;
262+
// gnss->lat = p.pvt.lat;
263+
// gnss->height_ellipsoid = p.pvt.height;
264+
// gnss->height_msl = p.pvt.hMSL;
265+
// gnss->h_acc = p.pvt.hAcc;
266+
// gnss->v_acc = p.pvt.vAcc;
267+
// gnss->vel_n = p.pvt.velN;
268+
// gnss->vel_e = p.pvt.velE;
269+
// gnss->vel_d = p.pvt.velD;
270+
// gnss->mag_var = p.pvt.magDec;
271+
// gnss->ground_speed = p.pvt.gSpeed;
272+
// gnss->course = p.pvt.headMot;
273+
// gnss->course_accy = p.pvt.headAcc;
274+
// gnss->vel_n = p.pvt.velN;
275+
// gnss->vel_e = p.pvt.velE;
276+
// gnss->vel_d = p.pvt.velD;
277+
// gnss->speed_accy = p.pvt.sAcc;
278+
// gnss->mag_var = p.pvt.magDec;
279+
// gnss->valid = ((p.pvt.flags & 0x01) != 0);
280+
// gnss->num_sat = p.pvt.numSV;
281+
// gnss->dop = p.pvt.pDOP;
282+
// gnss->fix_type = p.pvt.fixType;
283+
//
284+
// // These are not available from standard NMEA messages
285+
// // from ublox Class 0x01, ID 0x20
286+
// gnss->ecef.x = p.ecefp.ecefX;
287+
// gnss->ecef.y = p.ecefp.ecefY;
288+
// gnss->ecef.z = p.ecefp.ecefZ;
289+
// gnss->ecef.p_acc = p.ecefp.pAcc;
290+
// // from ublox Class 0x01, ID 0x11
291+
// gnss->ecef.vx = p.ecefv.ecefVX;
292+
// gnss->ecef.vy = p.ecefv.ecefVY;
293+
// gnss->ecef.vz = p.ecefv.ecefVZ;
294+
// gnss->ecef.s_acc = p.ecefv.sAcc;
295+
//
296+
// return true;
297+
// }
298+
//
299+
// return false;
300+
//}
301+
242302
///////////////////////////////////////////////////////////////////////////////////////////////
243303
// GNSS
244304
bool Varmint::gnss_read(rosflight_firmware::GnssStruct * gnss)
@@ -249,53 +309,20 @@ bool Varmint::gnss_read(rosflight_firmware::GnssStruct * gnss)
249309
gnss->header = p.header;
250310
gnss->pps = p.pps;
251311
gnss->unix_seconds = p.unix_seconds; // Unix time
252-
gnss->t_acc = p.pvt.tAcc;
253-
gnss->time_of_week = p.pvt.iTOW;
254-
gnss->year = p.pvt.year;
255-
gnss->month = p.pvt.month;
256-
gnss->day = p.pvt.day;
257-
gnss->hour = p.pvt.hour;
258-
gnss->min = p.pvt.min;
259-
gnss->sec = p.pvt.sec;
260-
gnss->nano = p.pvt.nano;
261-
gnss->lon = p.pvt.lon;
262-
gnss->lat = p.pvt.lat;
263-
gnss->height_ellipsoid = p.pvt.height;
264-
gnss->height_msl = p.pvt.hMSL;
265-
gnss->h_acc = p.pvt.hAcc;
266-
gnss->v_acc = p.pvt.vAcc;
267-
gnss->vel_n = p.pvt.velN;
268-
gnss->vel_e = p.pvt.velE;
269-
gnss->vel_d = p.pvt.velD;
270-
gnss->mag_var = p.pvt.magDec;
271-
gnss->ground_speed = p.pvt.gSpeed;
272-
gnss->course = p.pvt.headMot;
273-
gnss->course_accy = p.pvt.headAcc;
274-
gnss->vel_n = p.pvt.velN;
275-
gnss->vel_e = p.pvt.velE;
276-
gnss->vel_d = p.pvt.velD;
277-
gnss->speed_accy = p.pvt.sAcc;
278-
gnss->mag_var = p.pvt.magDec;
279-
gnss->valid = ((p.pvt.flags & 0x01) != 0);
280-
gnss->num_sat = p.pvt.numSV;
281-
gnss->dop = p.pvt.pDOP;
312+
gnss->unix_nanos = p.pvt.nano;
282313
gnss->fix_type = p.pvt.fixType;
283-
284-
// These are not available from standard NMEA messages
285-
// from ublox Class 0x01, ID 0x20
286-
gnss->ecef.x = p.ecefp.ecefX;
287-
gnss->ecef.y = p.ecefp.ecefY;
288-
gnss->ecef.z = p.ecefp.ecefZ;
289-
gnss->ecef.p_acc = p.ecefp.pAcc;
290-
// from ublox Class 0x01, ID 0x11
291-
gnss->ecef.vx = p.ecefv.ecefVX;
292-
gnss->ecef.vy = p.ecefv.ecefVY;
293-
gnss->ecef.vz = p.ecefv.ecefVZ;
294-
gnss->ecef.s_acc = p.ecefv.sAcc;
295-
314+
gnss->num_sat = p.pvt.numSV;
315+
gnss->lon = (double)p.pvt.lon* 1e-7; // Convert 100's of nanodegs into deg (DDS format)
316+
gnss->lat = (double)p.pvt.lat* 1e-7; // Convert 100's of nanodegs into deg (DDS format)
317+
gnss->height_msl = (float)p.pvt.hMSL* 1e-3; //mm to m
318+
gnss->vel_n = (float)p.pvt.velN* 1e-3; // mm/s to m/s
319+
gnss->vel_e = (float)p.pvt.velE* 1e-3; // mm/s to m/s
320+
gnss->vel_d = (float)p.pvt.velD* 1e-3; // mm/s to m/s
321+
gnss->h_acc = (float)p.pvt.hAcc* 1e-3; //mm to m
322+
gnss->v_acc = (float)p.pvt.vAcc* 1e-3; //mm to m
323+
gnss->speed_accy = (float)p.pvt.sAcc* 1e-3; // mm/s to m/s
296324
return true;
297325
}
298-
299326
return false;
300327
}
301328

‎boards/varmint_h7/varmint_10X/specific/BoardConfig.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@
4242

4343
#define SANDBOX false
4444
#define BOARD_STATUS_PRINT false
45-
#define USE_TELEM 0 // 1 = use UART, 0 = use VCP for link to companion computer.
45+
#define USE_TELEM 1 // 1 = use UART, 0 = use VCP for link to companion computer.
4646

4747
// UART used for printf's
4848
#define MISC_HUART (&huart2)

‎build_all.sh‎

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
#!/bin/bash
2+
3+
# Building the projects:
4+
# mkdir varmint_build && cd varmint_build && cmake .. -DBOARD_TO_BUILD=varmint && make -j
5+
# mkdir pixracer_pro_build && cd pixracer_pro_build && cmake .. -DBOARD_TO_BUILD=pixracer_pro && make -j
6+
#
7+
# alternate for make -j above is cmake --build . -j (note only one .)
8+
# make -j OR cmake --build . -j
9+
# to clean:
10+
# make clean -j OR make --build . --target clean -j
11+
#
12+
# unit test:
13+
# build:
14+
# mkdir test_build && cd test_build && cmake .. -DBOARD_TO_BUILD=test -DCMAKE_BUILD_TYPE=Release && make -j
15+
# test:
16+
# ./test/unit_tests
17+
18+
#!/bin/bash
19+
20+
# Save the current directory
21+
START_DIR=$(pwd)
22+
23+
# Create the build directory if it doesn't exist
24+
mkdir -p build
25+
26+
# Empty build/test directory if it exists, otherwise create it
27+
if [ -d "build/test" ]; then
28+
sudo rm -rf build/test/*
29+
else
30+
mkdir -p build/test
31+
fi
32+
33+
# Navigate to build/test directory
34+
cd build/test
35+
36+
# Run cmake with specified parameters and build project in Release mode
37+
cmake ../.. -DBOARD_TO_BUILD=test -DCMAKE_BUILD_TYPE=Release && make -j
38+
39+
# Run unit tests
40+
./test/unit_tests
41+
42+
# Return to starting directory
43+
cd "$START_DIR"
44+
45+
# Build varmint project
46+
# Empty build/varmint directory if it exists, otherwise create it
47+
if [ -d "build/varmint" ]; then
48+
sudo rm -rf build/varmint/*
49+
else
50+
mkdir -p build/varmint
51+
fi
52+
53+
# Navigate to build/varmint directory
54+
cd build/varmint
55+
56+
# Run cmake with specified parameters and build project
57+
cmake ../.. -DBOARD_TO_BUILD=varmint && make -j
58+
59+
# Return to starting directory
60+
cd "$START_DIR"
61+
62+
mv build/varmint/boards/varmint_h7/varmint/{*.elf,*.hex,*.bin} "$START_DIR/build/"
63+
64+
65+
# Build pixracer project
66+
# Empty build/pixracer_pro directory if it exists, otherwise create it
67+
if [ -d "build/pixracer_pro" ]; then
68+
sudo rm -rf build/pixracer_pro/*
69+
else
70+
mkdir -p build/pixracer_pro
71+
fi
72+
73+
# Navigate to build/pixracer directory
74+
cd build/pixracer_pro
75+
76+
# Run cmake with specified parameters and build project
77+
cmake ../.. -DBOARD_TO_BUILD=pixracer_pro && make -j
78+
79+
# Return to starting directory
80+
cd "$START_DIR"
81+
82+
mv build/pixracer_pro/boards/varmint_h7/pixracer_pro/{*.elf,*.hex,*.bin} "$START_DIR/build/"
83+
84+
85+

‎build_varmint.sh‎

Lines changed: 0 additions & 18 deletions
This file was deleted.

‎comms/mavlink/mavlink.cpp‎

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -143,25 +143,25 @@ void Mavlink::send_imu(uint8_t system_id, uint64_t timestamp_us, const turbomath
143143
send_message(msg, 0);
144144
}
145145

146-
void Mavlink::send_gnss(uint8_t system_id, const GnssStruct & data)
146+
void Mavlink::send_gnss(uint8_t system_id, GnssStruct * data)
147147
{
148148
mavlink_message_t msg;
149149
mavlink_msg_rosflight_gnss_pack(
150150
system_id, compid_, &msg,
151-
data.unix_seconds, // Unix time in seconds
152-
data.nano, // nanoseconds
153-
data.fix_type,
154-
data.num_sat,
155-
(double)data.lat * 1e-7, // Convert 100's of nanodegs into deg (DDS format)
156-
(double)data.lon * 1e-7, // Convert 100's of nanodegs into deg (DDS format)
157-
(float)data.height_msl * 1e-3, // mm to m
158-
(float)data.vel_n * 1e-3, // mm/s to m/s
159-
(float)data.vel_e * 1e-3, // mm/s to m/s
160-
(float)data.vel_d * 1e-3, // mm/s to m/s
161-
(float)data.h_acc * 1e-3, // mm/s to m/s
162-
(float)data.v_acc * 1e-3, // mm/s to m/s
163-
(float)data.speed_accy * 1e-3, // mm/s to m/s
164-
(float)data.header.timestamp);
151+
data->unix_seconds, // Unix time in seconds
152+
data->unix_nanos, // Unix nanoseconds
153+
data->fix_type,
154+
data->num_sat,
155+
data->lat,
156+
data->lon,
157+
data->height_msl,
158+
data->vel_n,
159+
data->vel_e,
160+
data->vel_d,
161+
data->h_acc,
162+
data->v_acc,
163+
data->speed_accy,
164+
data->header.timestamp);
165165
send_message(msg);
166166
}
167167

‎comms/mavlink/mavlink.h‎

Lines changed: 1 addition & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -69,10 +69,6 @@ class Mavlink : public CommLinkInterface
6969
const turbomath::Vector & gyro, float temperature) override;
7070
void send_log_message(uint8_t system_id, LogSeverity severity, const char * text) override;
7171
void send_mag(uint8_t system_id, const turbomath::Vector & mag) override;
72-
// void send_named_value_int(uint8_t system_id, uint32_t timestamp_ms, const char * const name,
73-
// int32_t value) override;
74-
// void send_named_value_float(uint8_t system_id, uint32_t timestamp_ms, const char * const name,
75-
// float value) override;
7672
void send_output_raw(uint8_t system_id, uint32_t timestamp_ms,
7773
const float raw_outputs[14]) override;
7874
void send_param_value_int(uint8_t system_id, uint16_t index, const char * const name,
@@ -88,7 +84,7 @@ class Mavlink : public CommLinkInterface
8884
int16_t loop_time_us) override;
8985
void send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1) override;
9086
void send_version(uint8_t system_id, const char * const version) override;
91-
void send_gnss(uint8_t system_id, const GnssStruct & data) override;
87+
void send_gnss(uint8_t system_id, GnssStruct * data) override;
9288
void send_error_data(uint8_t system_id, const StateManager::BackupData & error_data) override;
9389
void send_battery_status(uint8_t system_id, float voltage, float current) override;
9490

‎include/comm_link.h‎

Lines changed: 1 addition & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -145,10 +145,6 @@ class CommLinkInterface
145145
const turbomath::Vector & gyro, float temperature) = 0;
146146
virtual void send_log_message(uint8_t system_id, LogSeverity severity, const char * text) = 0;
147147
virtual void send_mag(uint8_t system_id, const turbomath::Vector & mag) = 0;
148-
// virtual void send_named_value_int(uint8_t system_id, uint32_t timestamp_ms,
149-
// const char * const name, int32_t value) = 0;
150-
// virtual void send_named_value_float(uint8_t system_id, uint32_t timestamp_ms,
151-
// const char * const name, float value) = 0;
152148
virtual void send_output_raw(uint8_t system_id, uint32_t timestamp_ms,
153149
const float raw_outputs[14]) = 0;
154150
virtual void send_param_value_int(uint8_t system_id, uint16_t index, const char * const name,
@@ -165,7 +161,7 @@ class CommLinkInterface
165161
int16_t num_errors, int16_t loop_time_us) = 0;
166162
virtual void send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1) = 0;
167163
virtual void send_version(uint8_t system_id, const char * const version) = 0;
168-
virtual void send_gnss(uint8_t system_id, const GnssStruct & data) = 0;
164+
virtual void send_gnss(uint8_t system_id, GnssStruct *data) = 0;
169165
virtual void send_error_data(uint8_t system_id, const StateManager::BackupData & error_data) = 0;
170166
virtual void send_battery_status(uint8_t system_id, float voltage, float current) = 0;
171167

0 commit comments

Comments
 (0)