Skip to content

Commit

Permalink
performance GPS driver
Browse files Browse the repository at this point in the history
  • Loading branch information
ClemensElflein committed Nov 8, 2024
1 parent 3376242 commit 2221cd7
Show file tree
Hide file tree
Showing 7 changed files with 182 additions and 133 deletions.
8 changes: 0 additions & 8 deletions boards/XCORE/board_ex.h
Original file line number Diff line number Diff line change
Expand Up @@ -59,14 +59,6 @@
// 255.255.255.0
#define FALLBACK_NETMASK 0xFFFFFF00

// Flash information for the bootloader
#define BOOT_ADDRESS 0x8020000
// Available flash pages for user program
#define FLASH_PAGE_COUNT 7
// Size of each flash page in bytes
#define FLASH_PAGE_SIZE_BYTES 0x20000
// Size of flash memory for the user program
#define PROGRAM_FLASH_SIZE_BYTES (FLASH_PAGE_COUNT * FLASH_PAGE_SIZE_BYTES)

#define SPID_IMU SPID3
#ifndef __ASSEMBLER__
Expand Down
1 change: 1 addition & 0 deletions main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,7 @@ int main(void) {

// chThdCreateStatic(testWa, sizeof(testWa), NORMALPRIO, testThreadFunc, NULL);
gps.set_state_callback(etl::delegate<void(const xbot::driver::gps::GpsInterface::GpsState &new_state)>::create<gpsUpdated>());
gps.set_datum(49.0, 10.0, 150);
gps.start_driver(&UARTD6, 921600);

// Subscribe to global events and dispatch to our services
Expand Down
8 changes: 8 additions & 0 deletions src/boot_service_discovery.c
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,14 @@ static void multicast_sender_thread(void *p) {
recvfrom(sockfd, boardAdvertisementRequestBuffer,
sizeof(boardAdvertisementRequestBuffer) - 1, 0, NULL, NULL);
if (received > 0) {
// check, if the command was "RESET", if so, we reset
if(strncmp(boardAdvertisementBuffer, "RESET", sizeof(boardAdvertisementBuffer)) == 0) {
// Set the boot address to bootloader and reset the system
MODIFY_REG(SYSCFG->UR2, SYSCFG_UR2_BOOT_ADD0,
(0x8000000 >> 16) << SYSCFG_UR2_BOOT_ADD0_Pos);
NVIC_SystemReset();
while(1);
}
// Send the multicast message
sendto(sockfd, boardAdvertisementBuffer, strlen(boardAdvertisementBuffer),
0, (struct sockaddr *)&multicast_addr, sizeof(multicast_addr));
Expand Down
67 changes: 56 additions & 11 deletions src/drivers/gps/gps_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include "gps_interface.h"

#include <etl/algorithm.h>
#include <ulog.h>

#include <cmath>

Expand All @@ -14,18 +15,26 @@ namespace gps {

GpsInterface::GpsInterface() {
datum_lat_ = datum_long_ = NAN;
chMtxObjectInit(&processing_mutex);
}
bool GpsInterface::start_driver(UARTDriver *uart, uint32_t baudrate) {
chDbgAssert(stopped_, "don't start the driver twice");
chDbgAssert(uart != nullptr, "need to provide a driver");
if(!stopped_) {
return false;
}
stopped_ = false;
this->uart = uart;
uart_config.speed = baudrate;
chThdCreateStatic(&wa, sizeof(wa), NORMALPRIO, GpsInterface::threadHelper, this);
return uartStart(uart, &uart_config) == MSG_OK;
bool uartStarted = uartStart(uart, &uart_config) == MSG_OK;
if(!uartStarted) {
return false;
}

stopped_ = false;
processing_thread_ = chThdCreateStatic(&wa, sizeof(wa), NORMALPRIO, threadHelper, this);
// Allow higher prio in order to not lose buffers. Active time in this thread is very minimal (it's just swapping buffers)
chThdCreateStatic(&recvThdWa, sizeof(recvThdWa), NORMALPRIO+1, receivingThreadHelper, this);
return true;
}

void GpsInterface::set_state_callback(const GpsInterface::StateCallback &function) {
Expand All @@ -45,23 +54,59 @@ bool GpsInterface::send_raw(const void *data, size_t size) {
}

void GpsInterface::threadFunc() {
size_t bytes_to_process = 1;
// size_t bytes_to_process = 1;
while(!stopped_) {
// Wait for data to arrive
chEvtWaitAny(ALL_EVENTS);
// Lock the processing buffer and process the data
chMtxLock(&processing_mutex);
// try to read as much as possible in order to not interrupt on single bytes
size_t read = sizeof(gps_buffer);
uartReceiveTimeout(uart, &read, gps_buffer, TIME_MS2I(100));
size_t processed = 0;
while(processed < read) {
size_t in = etl::min(bytes_to_process, read-processed);
bytes_to_process = process_bytes(gps_buffer+processed, in);
processed+=in;
// size_t processed = 0;
// while(processed < processing_buffer_len) {
// const size_t in = etl::min(bytes_to_process, processing_buffer_len-processed);
// bytes_to_process = process_bytes(processing_buffer+processed, in);
// processed+=in;
// }
process_bytes(processing_buffer, processing_buffer_len);
chMtxUnlock(&processing_mutex);
}
}


void GpsInterface::receivingThreadFunc() {
while(!stopped_) {
// try to read as much as possible in order to not interrupt on single bytes
size_t read = RECV_BUFFER_SIZE;
// Get the pointer to the receiving buffer (it's not the processing buffer)
uint8_t* buffer = (processing_buffer == recv_buffer1) ? recv_buffer2 : recv_buffer1;
memset(buffer, 0, RECV_BUFFER_SIZE);
uartReceiveTimeout(uart, &read, buffer, TIME_S2I(1));
// swap buffers and instantly restart DMA
bool locked = chMtxTryLock(&processing_mutex);
if(!locked) {
ULOG_WARNING("GPS processing too slow to keep up with incoming data");
chMtxLock(&processing_mutex);
}
// Swap the buffers
processing_buffer = buffer;
processing_buffer_len = read;
chMtxUnlock(&processing_mutex);
// Signal the processing thread
if(processing_thread_) {
chEvtSignal(processing_thread_, 1);
}
}
}


void GpsInterface::threadHelper(void *instance) {
auto *gps_interface = static_cast<GpsInterface *>(instance);
gps_interface->threadFunc();
}
void GpsInterface::receivingThreadHelper(void *instance) {
auto *gps_interface = static_cast<GpsInterface *>(instance);
gps_interface->receivingThreadFunc();
}

void GpsInterface::send_rtcm(const uint8_t *data, size_t size) {
send_raw(data, size);
Expand Down
37 changes: 20 additions & 17 deletions src/drivers/gps/gps_interface.h
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
#define OPEN_MOWER_ROS_GPS_INTERFACE_H

#include <etl/delegate.h>

#include "ch.h"
#include "hal.h"

Expand All @@ -22,13 +23,7 @@ class GpsInterface {
* The final GPS state we're interested in.
*/
struct GpsState {
enum FixType {
NO_FIX = 0,
DR_ONLY = 1,
FIX_2D = 2,
FIX_3D = 3,
GNSS_DR_COMBINED = 4
};
enum FixType { NO_FIX = 0, DR_ONLY = 1, FIX_2D = 2, FIX_3D = 3, GNSS_DR_COMBINED = 4 };

enum RTKType { RTK_NONE = 0, RTK_FLOAT = 1, RTK_FIX = 2 };

Expand Down Expand Up @@ -65,7 +60,7 @@ class GpsInterface {
typedef etl::delegate<void(const GpsState &new_state)> StateCallback;

public:
bool start_driver(UARTDriver* uart, uint32_t baudrate);
bool start_driver(UARTDriver *uart, uint32_t baudrate);
void set_state_callback(const GpsInterface::StateCallback &function);

void set_datum(double datum_lat, double datum_long, double datum_height);
Expand Down Expand Up @@ -94,21 +89,29 @@ class GpsInterface {
virtual size_t process_bytes(const uint8_t *buffer, size_t len) = 0;

private:
uint8_t gps_buffer[512]{};
uint8_t gps_buffer2[512]{};
uint8_t current_buffer = 0;
size_t gps_buffer_count = 0;
size_t requested_bytes = 0;
static constexpr size_t RECV_BUFFER_SIZE=512;
// Keep two buffers for streaming data while doing processing
uint8_t recv_buffer1[RECV_BUFFER_SIZE]{};
uint8_t recv_buffer2[RECV_BUFFER_SIZE]{};
uint8_t* processing_buffer = recv_buffer1;
size_t processing_buffer_len = 0;
UARTDriver *uart{};
UARTConfig uart_config{};
UARTConfig uart_config{};

THD_WORKING_AREA(wa, 1024);
THD_WORKING_AREA(wa, 1024){};
THD_WORKING_AREA(recvThdWa, 100){};
thread_t* processing_thread_ = nullptr;
// mutex held while processing data
// i.e. receiving thread cannot swap buffers when this is held
ch_mutex processing_mutex{};

bool stopped_ = true;

void threadFunc();
void threadFunc();
void receivingThreadFunc();

static void threadHelper(void* instance);
static void threadHelper(void *instance);
static void receivingThreadHelper(void *instance);
};
} // namespace gps
} // namespace driver
Expand Down
Loading

0 comments on commit 2221cd7

Please sign in to comment.