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

calibrate MPU and make GPS work

Browse Source
This commit is contained in:
Alex Mikhalev 2019-01-30 19:50:51 -08:00
parent 0a6305c52a
commit 1a39252b2e
5 changed files with 74 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

100
main/ugv_io_gps.cc
View File

@ -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:
}
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) {
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) {
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);
BaseType_t pdRet = xQueueReceive(uart_queue_, &uart_event, portMAX_DELAY);
if (!pdRet) {
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
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)
}
if (lines_received == 0) {
ESP_LOGW(TAG, "no lines received, waiting for more data");
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;
}
}
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
View File

@ -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
View File

@ -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() {
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
View File

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

4
main/ugv_main.cc
View File

@ -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;
}