mirror of
https://github.com/peter-tanner/des-car-2022.git
synced 2024-11-30 09:00:21 +08:00
39 lines
993 B
C++
39 lines
993 B
C++
|
|
||
|
|
#include <stdint.h>
|
||
|
|
#include <NeoSWSerial.h>
|
||
|
|
|
||
|
|
// https://stackoverflow.com/a/24396562
|
||
|
|
// Very nice angle normalization
|
||
|
|
inline int16_t normalize_angle(int16_t angle)
|
||
|
|
{
|
||
|
|
// if (angle > 180)
|
||
|
|
// angle -= 360;
|
||
|
|
// else if (angle < -180)
|
||
|
|
// angle += 360;
|
||
|
|
// return angle;
|
||
|
|
angle %= 360;
|
||
|
|
int16_t fix = angle / 180; // Integer division!!
|
||
|
|
return (fix) ? angle - (360 * (fix)) : angle;
|
||
|
|
}
|
||
|
|
|
||
|
|
#ifdef FAST_GNSS
|
||
|
|
// https://github.com/SlashDevin/NeoGPS/blob/master/examples/ubloxRate/ubloxRate.ino
|
||
|
|
inline void send_ubx(NeoSWSerial *gps_port, const unsigned char *progmemBytes, uint16_t len)
|
||
|
|
{
|
||
|
|
gps_port->write(0xB5); // SYNC1
|
||
|
|
gps_port->write(0x62); // SYNC2
|
||
|
|
|
||
|
|
uint8_t a = 0, b = 0;
|
||
|
|
while (len-- > 0)
|
||
|
|
{
|
||
|
|
uint8_t c = pgm_read_byte(progmemBytes++);
|
||
|
|
a += c;
|
||
|
|
b += a;
|
||
|
|
gps_port->write(c);
|
||
|
|
}
|
||
|
|
|
||
|
|
gps_port->write(a); // CHECKSUM A
|
||
|
|
gps_port->write(b); // CHECKSUM B
|
||
|
|
|
||
|
|
} // sendUBX
|
||
|
|
#endif
|