amikhalev
/
uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

calibrate MPU and make GPS work

try-fix-comms-errors
Alex Mikhalev 6 years ago
parent
0a6305c52a
commit
1a39252b2e
5 changed files with 74 additions and 51 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 98
      main/ugv_io_gps.cc
  2. 6
      main/ugv_io_gps.hh
  3. 14
      main/ugv_io_mpu.cc
  4. 1
      main/ugv_io_mpu.hh
  5. 4
      main/ugv_main.cc

98
main/ugv_io_gps.cc
Unescape View File

@ -65,14 +65,19 @@ void UART_GPS::Init() { @@ -65,14 +65,19 @@ void UART_GPS::Init() {
uart_set_pin(GPS_UART, GPS_UART_TX_PIN, GPS_UART_RX_PIN, UART_PIN_NO_CHANGE,
UART_PIN_NO_CHANGE);
ret = uart_driver_install(GPS_UART, GPS_UART_RX_BUF_SIZE,
GPS_UART_TX_BUF_SIZE, 0, NULL, 0);
GPS_UART_TX_BUF_SIZE, 8, &this->uart_queue_, 0);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "uart_driver_install: %d", ret);
return;
}
uart_enable_pattern_det_intr(GPS_UART, '\n', 1, 10000, 10, 10);
uart_pattern_queue_reset(GPS_UART, 8);
uart_flush(GPS_UART);
ESP_LOGI(TAG, "gps uart configured");
BaseType_t xRet = xTaskCreate(UART_GPS::GPS_Task, "ugv_io_gps", 2 * 1024,
buffer_ = new uint8_t[GPS_BUF_SIZE];
BaseType_t xRet = xTaskCreate(UART_GPS::TaskEntry, "ugv_io_gps", 8 * 1024,
this, 3, &this->task_);
if (xRet != pdTRUE) {
ESP_LOGE(TAG, "error creating GPS task");
@ -102,7 +107,6 @@ err: @@ -102,7 +107,6 @@ err:
}
void UART_GPS::ProcessLine(const char *line, size_t len) {
ESP_LOGI(TAG, "gps data: %.*s", len, line);
enum minmea_sentence_id id = minmea_sentence_id(line, false);
switch (id) {
case MINMEA_INVALID:
@ -123,7 +127,11 @@ void UART_GPS::ProcessLine(const char *line, size_t len) { @@ -123,7 +127,11 @@ void UART_GPS::ProcessLine(const char *line, size_t len) {
data_.speed = minmea_tofloat(&rmc.speed);
data_.course = minmea_tofloat(&rmc.course);
data_.last_update = xTaskGetTickCount();
ESP_LOGD(TAG, "RMC: %s, (%f,%f), speed=%f, course=%f",
data_.valid ? "valid" : "invalid", data_.latitude,
data_.longitude, data_.speed, data_.course);
xSemaphoreGive(mutex_);
break;
}
case MINMEA_SENTENCE_GGA: {
minmea_sentence_gga gga;
@ -137,75 +145,71 @@ void UART_GPS::ProcessLine(const char *line, size_t len) { @@ -137,75 +145,71 @@ void UART_GPS::ProcessLine(const char *line, size_t len) {
data_.num_satellites = gga.satellites_tracked;
data_.latitude = minmea_tofloat(&gga.latitude);
data_.longitude = minmea_tofloat(&gga.longitude);
if (gga.altitude_units != 'M') {
if (gga.fix_quality != 0 && gga.altitude_units != 'M') {
ESP_LOGW(TAG, "unknown altitude units: %c", gga.altitude_units);
}
data_.altitude = minmea_tofloat(&gga.altitude);
data_.last_update = xTaskGetTickCount();
ESP_LOGD(TAG, "GGA: qual: %d, num_sat=%d, (%f,%f), alt=%f",
data_.fix_quality, data_.num_satellites, data_.latitude,
data_.longitude, data_.altitude);
xSemaphoreGive(mutex_);
break;
}
default: ESP_LOGW(TAG, "unsupported nmea sentence: %s", line);
default: ESP_LOGV(TAG, "unsupported nmea sentence: %s", line);
}
}
void UART_GPS::GPS_Task(void *arg) {
UART_GPS *gps = (UART_GPS *)arg;
(void)gps;
void UART_GPS::HandleUartPattern() {
int pos = uart_pattern_pop_pos(GPS_UART);
if (pos < 0) {
ESP_LOGE(TAG, "uart_pattern_pop_pos: %d", pos);
return;
}
int read_bytes =
uart_read_bytes(GPS_UART, this->buffer_, pos + 1, pdMS_TO_TICKS(100));
if (read_bytes <= 0) {
ESP_LOGW(TAG, "uart_read_bytes error: %d", read_bytes);
return;
}
ESP_LOGV(TAG, "GPS bytes received: %.*s", read_bytes, buffer_);
buffer_[read_bytes] = '\0';
this->ProcessLine((const char *)buffer_,
read_bytes + 1); // process that line
}
void UART_GPS::DoTask() {
ESP_LOGI(TAG, "gps_task start");
char * buf1 = (char *)malloc(GPS_BUF_SIZE);
char * buf2 = (char *)malloc(GPS_BUF_SIZE);
char * buf_end = buf1;
size_t read_bytes;
uart_event_t uart_event;
esp_err_t ret;
ret = gps->WriteCommand(NMEA_OUTPUT_RMCGGA, sizeof(NMEA_OUTPUT_RMCGGA));
ret = WriteCommand(NMEA_OUTPUT_RMCGGA, sizeof(NMEA_OUTPUT_RMCGGA));
ESP_LOGI(TAG, "sent output rmc and gga: %d", ret);
ret = gps->WriteCommand(NMEA_UPDATE_1HZ, sizeof(NMEA_UPDATE_1HZ));
ret = WriteCommand(NMEA_UPDATE_1HZ, sizeof(NMEA_UPDATE_1HZ));
ESP_LOGI(TAG, "sent update 1hz: %d", ret);
vTaskDelay(pdMS_TO_TICKS(100));
ret = gps->WriteCommand(NMEA_Q_RELEASE, sizeof(NMEA_Q_RELEASE));
ret = WriteCommand(NMEA_Q_RELEASE, sizeof(NMEA_Q_RELEASE));
ESP_LOGI(TAG, "sent q release: %d", ret);
while (true) {
size_t buf_remaining = GPS_BUF_SIZE - (buf_end - buf1);
read_bytes = uart_read_bytes(GPS_UART, (uint8_t *)buf_end, buf_remaining,
portMAX_DELAY);
if (read_bytes <= 0) {
ESP_LOGW(TAG, "GPS error: %d", read_bytes);
continue;
}
ESP_LOGW(TAG, "GPS bytes received: %.*s", read_bytes, buf_end);
buf_end += read_bytes;
int lines_received = 0;
for (char *c = buf1; c < buf_end; c++) {
if (*c != '\n') { // wait for \n, if not found loop again
BaseType_t pdRet = xQueueReceive(uart_queue_, &uart_event, portMAX_DELAY);
if (!pdRet) {
continue;
}
lines_received++;
c++; // now c is one past the end of the whole string including \n
size_t remaining = buf_end - c;
memcpy(buf2, c, remaining); // copy remaining text from buf1 to buf2
buf_end = buf2 + remaining; // update buf_end to point to the end of the
// remaining text in buf2
char * str_ptr = buf1;
size_t str_len = c - buf1;
str_ptr[str_len] =
'\0'; // append the NULL byte (safe because old data was copied)
gps->ProcessLine(str_ptr, str_len); // process that line
std::swap(buf1, buf2); // swap buffers
c = buf1; // start over on text which was copied to buf2 (now buf1)
switch (uart_event.type) {
case UART_DATA: break;
case UART_PATTERN_DET: this->HandleUartPattern(); break;
default:
ESP_LOGW(TAG, "Unhandled GPS event type: %d", uart_event.type);
break;
}
if (lines_received == 0) {
ESP_LOGW(TAG, "no lines received, waiting for more data");
}
}
free(buf1);
free(buf2);
void UART_GPS::TaskEntry(void *arg) {
UART_GPS *gps = (UART_GPS *)arg;
gps->DoTask();
vTaskDelete(NULL);
}

6
main/ugv_io_gps.hh
Unescape View File

@ -45,12 +45,16 @@ class UART_GPS { @@ -45,12 +45,16 @@ class UART_GPS {
private:
TaskHandle_t task_;
SemaphoreHandle_t mutex_;
QueueHandle_t uart_queue_;
GpsData data_;
uint8_t* buffer_;
esp_err_t WriteCommand(const char* cmd, size_t len);
void HandleUartPattern();
void ProcessLine(const char* line, size_t len);
static void GPS_Task(void* arg);
void DoTask();
static void TaskEntry(void* arg);
};
} // namespace io

14
main/ugv_io_mpu.cc
Unescape View File

@ -51,6 +51,7 @@ void MPU::Init() { @@ -51,6 +51,7 @@ void MPU::Init() {
ESP_LOGE(TAG, "MPU initialization error");
return;
}
Calibrate();
mpu_->setAccelFullScale(MPU_ACCEL_FS);
mpu_->setGyroFullScale(MPU_GYRO_FS);
// force magnetometer into continuous mode
@ -60,6 +61,19 @@ void MPU::Init() { @@ -60,6 +61,19 @@ void MPU::Init() {
ESP_LOGI(TAG, "MPU initialized");
}
void MPU::Calibrate() {
mpud::raw_axes_t accel_offset, gyro_offset;
mpu_->computeOffsets(&accel_offset, &gyro_offset);
mpu_->setAccelOffset(accel_offset);
mpu_->setGyroOffset(gyro_offset);
{
auto ao = mpud::math::accelGravity(accel_offset, mpud::ACCEL_FS_2G);
auto go = mpud::math::gyroDegPerSec(gyro_offset, mpud::GYRO_FS_250DPS);
ESP_LOGI(TAG, "MPU offsets: accel=(%f, %f, %f) gyro=(%f, %f, %f)", ao.x,
ao.y, ao.z, go.x, go.y, go.z);
}
}
void MPU::GetData(MpuData &data) {
esp_err_t ret;
// uint8_t mxh, mxl, myh, myl, mzh, mzl, n;

1
main/ugv_io_mpu.hh
Unescape View File

@ -31,6 +31,7 @@ class MPU { @@ -31,6 +31,7 @@ class MPU {
~MPU();
void Init();
void Calibrate();
void GetData(MpuData &data);

4
main/ugv_main.cc
Unescape View File

@ -76,13 +76,13 @@ struct State { @@ -76,13 +76,13 @@ struct State {
ahrs_.update(g.x, g.y, g.z, a.x, a.y, a.z, m.x, m.y, m.z);
}
if (time_us >= last_print + 500 * 1000) { // 1s
ESP_LOGI(TAG,
ESP_LOGD(TAG,
"inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)",
inputs.mpu.accel.x, inputs.mpu.accel.y, inputs.mpu.accel.z,
inputs.mpu.gyro_rate.x, inputs.mpu.gyro_rate.y,
inputs.mpu.gyro_rate.z, inputs.mpu.mag.x, inputs.mpu.mag.y,
inputs.mpu.mag.z);
ESP_LOGI(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(),
ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(),
ahrs_.getPitch(), ahrs_.getRoll());
last_print = time_us;
}

Write Preview
Loading…
Cancel
Save