Momentum-Apps/non_catalog_apps/ublox/ublox_worker.c

#include "ublox_worker_i.h"

#define TAG "UbloxWorker"

UbloxWorker* ublox_worker_alloc() {
    UbloxWorker* ublox_worker = malloc(sizeof(UbloxWorker));

    ublox_worker->thread =
        furi_thread_alloc_ex("UbloxWorker", 2 * 1024, ublox_worker_task, ublox_worker);

    ublox_worker->callback = NULL;
    ublox_worker->context = NULL;

    ublox_worker_change_state(ublox_worker, UbloxWorkerStateReady);

    return ublox_worker;
}

void ublox_worker_free(UbloxWorker* ublox_worker) {
    furi_assert(ublox_worker);

    furi_thread_free(ublox_worker->thread);

    free(ublox_worker);
}

UbloxWorkerState ublox_worker_get_state(UbloxWorker* ublox_worker) {
    return ublox_worker->state;
}

void ublox_worker_start(
    UbloxWorker* ublox_worker,
    UbloxWorkerState state,
    UbloxWorkerCallback callback,
    void* context) {
    furi_assert(ublox_worker);

    ublox_worker->callback = callback;
    ublox_worker->context = context;

    ublox_worker_change_state(ublox_worker, state);
    furi_thread_start(ublox_worker->thread);
}

void ublox_worker_stop(UbloxWorker* ublox_worker) {
    furi_assert(ublox_worker);
    furi_assert(ublox_worker->thread);
    Ublox* ublox = ublox_worker->context;
    furi_assert(ublox);
    //FURI_LOG_I(TAG, "worker_stop: %p", ublox);

    /*FuriThreadState state = furi_thread_get_state(ublox_worker->thread);
  if (state == FuriThreadStateStopped) {
      FURI_LOG_I(TAG, "worker state stopped");
  } else if (state == FuriThreadStateStarting) {
      FURI_LOG_I(TAG, "worker state starting");
  } else if (state == FuriThreadStateRunning) {
      FURI_LOG_I(TAG, "worker state running");
      }*/

    // close the logfile if currently logging
    //FURI_LOG_I(TAG, "log state: %d", ublox->log_state);
    if(ublox->log_state == UbloxLogStateLogging) {
	FURI_LOG_I(TAG, "closing log file on worker stop");
	ublox->log_state = UbloxLogStateNone;
	if (!kml_close_file(&(ublox->kmlfile))) {
	    FURI_LOG_E(TAG, "failed to close KML file!");
	}
    }
    if(furi_thread_get_state(ublox_worker->thread) != FuriThreadStateStopped) {
        ublox_worker_change_state(ublox_worker, UbloxWorkerStateStop);
        furi_thread_join(ublox_worker->thread);
    }
}

void ublox_worker_change_state(UbloxWorker* ublox_worker, UbloxWorkerState state) {
    ublox_worker->state = state;
}

void clear_ublox_data() {
    int fails = 0;
    
    if (!furi_hal_i2c_is_device_ready(
	    &furi_hal_i2c_handle_external,
	    UBLOX_I2C_ADDRESS << 1,
	    furi_ms_to_ticks(I2C_TIMEOUT_MS))) {
	FURI_LOG_E(TAG, "clear_ublox_data(): device not ready");
	return;
    }
    
    uint8_t tx[] = {0xff};
    uint8_t response = 0;
    
    while(response != 0xff && fails < 30) {
        if(!furi_hal_i2c_trx(
               &furi_hal_i2c_handle_external,
               UBLOX_I2C_ADDRESS << 1,
               tx, 1,
               &response, 1,
               furi_ms_to_ticks(I2C_TIMEOUT_MS))) {
	    // if the GPS is disconnected during this loop, this will
	    // loop forever, we must make that not happen. 30 loops is
	    // plenty, and if the clearing doesn't work, the requisite
	    // error will be generated by the caller on the next
	    // actual attempt to reach the GPS.
	    fails++;
            FURI_LOG_E(TAG, "clear_ublox_data(): error clearing ublox data");
        }
    }
}


int32_t ublox_worker_task(void* context) {

    UbloxWorker* ublox_worker = context;

    furi_hal_i2c_acquire(&furi_hal_i2c_handle_external);
    
    if(ublox_worker->state == UbloxWorkerStateRead) {
	ublox_worker_read_nav_messages(context);
    } else if (ublox_worker->state == UbloxWorkerStateSyncTime) {
	FURI_LOG_I(TAG, "sync time");
	ublox_worker_sync_to_gps_time(ublox_worker);
    } else if(ublox_worker->state == UbloxWorkerStateResetOdometer) {
        ublox_worker_reset_odo(ublox_worker);
    } else if(ublox_worker->state == UbloxWorkerStateStop) {
        FURI_LOG_D(TAG, "state stop");
    }

    ublox_worker_change_state(ublox_worker, UbloxWorkerStateStop);

    furi_hal_i2c_release(&furi_hal_i2c_handle_external);

    return 0;
}

void ublox_worker_read_nav_messages(void* context) {
    // this function is fairly complicated: it inits the GPS, handles
    // logging states, and reads data from the GPS to push it to the
    // main app struct.
    
    // IMPORTANT NOTE: we don't use a timer that continually respawns
    // the thread because that causes a memory leak.
    UbloxWorker* ublox_worker = context;
    Ublox* ublox = ublox_worker->context;

    // We only start logging at the same time we restart the worker.
    if(ublox->log_state == UbloxLogStateStartLogging) {
	FURI_LOG_I(TAG, "start logging");
	// assemble full logfile pathname
	FuriString* fullname = furi_string_alloc();
	path_concat(furi_string_get_cstr(ublox->logfile_folder), ublox->text_store, fullname);
	FURI_LOG_I(TAG, "fullname is %s", furi_string_get_cstr(fullname));
	
	if (!kml_open_file(ublox->storage, &(ublox->kmlfile), furi_string_get_cstr(fullname))) {
	    FURI_LOG_E(TAG, "failed to open KML file %s!", furi_string_get_cstr(fullname));
	    ublox->log_state = UbloxLogStateNone;
	}
	ublox->log_state = UbloxLogStateLogging;
	furi_string_free(fullname);

	ublox_worker->callback(UbloxWorkerEventLogStateChanged, ublox_worker->context);
           
    }
    
    while(!ublox->gps_initted) {
	if(ublox_worker->state != UbloxWorkerStateRead) {
	    return;
	}

	// have to clear right before init to make retrying init work
	clear_ublox_data();
	if(ublox_worker_init_gps(ublox_worker)) {
	    ublox->gps_initted = true;
	    break;
	} else {
	    ublox_worker->callback(UbloxWorkerEventFailed, ublox_worker->context);
	    FURI_LOG_E(TAG, "init GPS failed, try again");
	}
	// don't try constantly, no reason to
	furi_delay_ms(500);
    }

    // clear data so we don't an error on startup
    clear_ublox_data();
    
    // break the loop when the thread state changes
    while(ublox_worker->state == UbloxWorkerStateRead) {
	// we interrupt with checking the state to help reduce
	// lag. it's not perfect, but it does overall improve things.
	bool pvt = ublox_worker_read_pvt(ublox_worker);
	
	if (ublox_worker->state != UbloxWorkerStateRead) break;
	// clearing makes the second read much faster
	clear_ublox_data();
	
	if (ublox_worker->state != UbloxWorkerStateRead) break;
	
	bool odo = ublox_worker_read_odo(ublox_worker);
	
	if (pvt && odo) {
	    ublox_worker->callback(UbloxWorkerEventDataReady, ublox_worker->context);
	    
	    if (ublox->log_state == UbloxLogStateLogging) {
		if (!kml_add_path_point(&(ublox->kmlfile),
					// ublox returns values as floats * 1e7 in int form
					(double)(ublox->nav_pvt.lat) / (double)1e7,
					(double)(ublox->nav_pvt.lon) / (double)1e7,
					ublox->nav_pvt.hMSL / 1e3)) { // convert altitude to meters
		    FURI_LOG_E(TAG, "failed to write line to file");
		}
	    }
	} else {
	    ublox_worker->callback(UbloxWorkerEventFailed, ublox_worker->context);
	}

	uint32_t ticks = furi_get_tick();
	while(furi_get_tick() - ticks < furi_ms_to_ticks(((ublox->data_display_state).refresh_rate * 1000))) {
	    // putting this here (should) make the logging response faster
	    if(ublox->log_state == UbloxLogStateStopLogging) {
		FURI_LOG_I(TAG, "stop logging");
		if (!kml_close_file(&(ublox->kmlfile))) {
		    FURI_LOG_E(TAG, "failed to close KML file!");
		}
		ublox->log_state = UbloxLogStateNone;
		ublox_worker->callback(UbloxWorkerEventLogStateChanged, ublox_worker->context);
	    }
	    if(ublox_worker->state != UbloxWorkerStateRead) {
		return;
	    }
	}
    }
}

void ublox_worker_sync_to_gps_time(void* context) {
    UbloxWorker* ublox_worker = context;
    Ublox* ublox = ublox_worker->context;

    UbloxFrame frame_tx;
    frame_tx.class = UBX_NAV_CLASS;
    frame_tx.id = UBX_NAV_TIMEUTC_MESSAGE;
    frame_tx.len = 0;
    frame_tx.payload = NULL;
    UbloxMessage* message_tx = ublox_frame_to_bytes(&frame_tx);


    UbloxMessage* message_rx = ublox_worker_i2c_transfer(message_tx, UBX_NAV_TIMEUTC_MESSAGE_LENGTH);
    if(message_rx == NULL) {
	FURI_LOG_E(TAG, "get_gps_time transfer failed");
	ublox_worker_change_state(ublox_worker, UbloxWorkerStateStop);
	ublox_worker->callback(UbloxWorkerEventFailed, ublox_worker->context);
	return;
    }
    FURI_LOG_I(TAG, "got message");
    UbloxFrame* frame_rx = ublox_bytes_to_frame(message_rx);
    ublox_message_free(message_rx);

    if(frame_rx == NULL) {
	FURI_LOG_E(TAG, "NULL pointer, something wrong with NAV-TIMEUTC message!");
	ublox_worker_change_state(ublox_worker, UbloxWorkerStateStop);
	ublox_worker->callback(UbloxWorkerEventFailed, ublox_worker->context);
	return;
    } else {
	Ublox_NAV_TIMEUTC_Message nav_timeutc = {
	    .iTOW = (frame_rx->payload[0]) | (frame_rx->payload[1] << 8) |
                    (frame_rx->payload[2] << 16) | (frame_rx->payload[3] << 24),
	    .tAcc = (frame_rx->payload[4]) | (frame_rx->payload[5] << 8) |
                    (frame_rx->payload[6] << 16) | (frame_rx->payload[7] << 24),
	    .nano = (frame_rx->payload[8]) | (frame_rx->payload[9] << 8) |
                    (frame_rx->payload[10] << 16) | (frame_rx->payload[11] << 24),
            .year = (frame_rx->payload[12]) | (frame_rx->payload[13] << 8),
            .month = frame_rx->payload[14],
            .day = frame_rx->payload[15],
            .hour = frame_rx->payload[16],
            .min = frame_rx->payload[17],
            .sec = frame_rx->payload[18],
            .valid = frame_rx->payload[19],
	};

	ublox->nav_timeutc = nav_timeutc;
	ublox_frame_free(frame_rx);
	ublox_worker->callback(UbloxWorkerEventDataReady, ublox_worker->context);
    }
}

FuriString* print_uint8_array(uint8_t* array, int length) {
    FuriString* s = furi_string_alloc();

    for(int i = 0; i < length - 1; i++) {
        furi_string_cat_printf(s, "%x, ", array[i]);
    }
    furi_string_cat_printf(s, "%x", array[length - 1]);

    return s;
}

UbloxMessage* ublox_worker_i2c_transfer(UbloxMessage* message_tx, uint8_t read_length) {

    if (!furi_hal_i2c_is_device_ready(
	    &furi_hal_i2c_handle_external,
	    UBLOX_I2C_ADDRESS << 1,
	    furi_ms_to_ticks(I2C_TIMEOUT_MS))) {
	FURI_LOG_E(TAG, "device not ready");
	return NULL;
    }

    // Either our I2C implementation is broken or the GPS's is, so we
    // end up reading a lot more data than we need to. That means that
    // the I2C comm code for this app is a little bit of a hack, but
    // it works fine and is fast enough, so I don't really care. It
    // certainly doesn't break the GPS.
    if(!furi_hal_i2c_tx(
           &furi_hal_i2c_handle_external,
           UBLOX_I2C_ADDRESS << 1,
           message_tx->message, message_tx->length,
           furi_ms_to_ticks(I2C_TIMEOUT_MS))) {
        FURI_LOG_E(TAG, "error writing message to GPS");
        return NULL;
    }
    uint8_t* response = malloc((size_t)read_length);
    // The GPS sends 0xff until it has a complete message to respond
    // with. We have to wait until it stops sending that. (Why this
    // works is a little bit...uh, well, I don't know. Shouldn't reading
    // more bytes make it so that the data is completely read out and no
    // longer available?)

    //FURI_LOG_I(TAG, "start ticks at %lu", furi_get_tick()); // returns ms
    while(true) {
	if(!furi_hal_i2c_rx(
	       &furi_hal_i2c_handle_external,
               UBLOX_I2C_ADDRESS << 1,
               response, 1,
               furi_ms_to_ticks(I2C_TIMEOUT_MS))) {
	    FURI_LOG_E(TAG, "error reading first byte of response");
            free(response);
            return NULL;
	}
	
        // checking with 0xb5 prevents strange bursts of junk data from becoming an issue.
        if(response[0] != 0xff && response[0] == 0xb5) {
	    //FURI_LOG_I(TAG, "read rest of message at %lu", furi_get_tick());
            if(!furi_hal_i2c_rx(
                   &furi_hal_i2c_handle_external,
                   UBLOX_I2C_ADDRESS << 1,
                   &(response[1]), read_length - 1, // first byte already read
                   furi_ms_to_ticks(I2C_TIMEOUT_MS))) {
                FURI_LOG_E(TAG, "error reading rest of response");
                free(response);
                return NULL;
            }
            break;
        }
	furi_delay_ms(1);
    }

    UbloxMessage* message_rx = malloc(sizeof(UbloxMessage));
    message_rx->message = response;
    message_rx->length = read_length;
    return message_rx; // message_rx->message needs to be freed later
}

bool ublox_worker_read_pvt(UbloxWorker* ublox_worker) {
    //FURI_LOG_I(TAG, "mem free before PVT read: %u", memmgr_get_free_heap());
    Ublox* ublox = ublox_worker->context;

    // Read NAV-PVT by sending NAV-PVT with no payload
    UbloxFrame* frame_tx = malloc(sizeof(UbloxFrame));
    frame_tx->class = UBX_NAV_CLASS;
    frame_tx->id = UBX_NAV_PVT_MESSAGE;
    frame_tx->len = 0;
    frame_tx->payload = NULL;
    UbloxMessage* message_tx = ublox_frame_to_bytes(frame_tx);
    ublox_frame_free(frame_tx);

    UbloxMessage* message_rx = ublox_worker_i2c_transfer(message_tx, UBX_NAV_PVT_MESSAGE_LENGTH);
    ublox_message_free(message_tx);
    if(message_rx == NULL) {
        FURI_LOG_E(TAG, "read_pvt transfer failed");
        //ublox_worker_change_state(ublox_worker, UbloxWorkerStateStop);
        return false;
    }

    UbloxFrame* frame_rx = ublox_bytes_to_frame(message_rx);
    ublox_message_free(message_rx);

    if(frame_rx == NULL) {
        FURI_LOG_E(TAG, "NULL pointer, something wrong with NAV-PVT message!");
        //ublox_worker_change_state(ublox_worker, UbloxWorkerStateStop);
        return false;
    } else {
        // build nav-pvt struct. this is very ugly and there's not much I can do about it.
        Ublox_NAV_PVT_Message nav_pvt = {
            .iTOW = (frame_rx->payload[0]) | (frame_rx->payload[1] << 8) |
                    (frame_rx->payload[2] << 16) | (frame_rx->payload[3] << 24),
            .year = (frame_rx->payload[4]) | (frame_rx->payload[5] << 8),
            .month = frame_rx->payload[6],
            .day = frame_rx->payload[7],
            .hour = frame_rx->payload[8],
            .min = frame_rx->payload[9],
            .sec = frame_rx->payload[10],
            .valid = frame_rx->payload[11],
            .tAcc = (frame_rx->payload[12]) | (frame_rx->payload[13] << 8) |
                    (frame_rx->payload[14] << 16) | (frame_rx->payload[15] << 24),
            .nano = (frame_rx->payload[16]) | (frame_rx->payload[17] << 8) |
                    (frame_rx->payload[18] << 16) | (frame_rx->payload[19] << 24),
            .fixType = frame_rx->payload[20],
            .flags = frame_rx->payload[21],
            .flags2 = frame_rx->payload[22],
            .numSV = frame_rx->payload[23],
            .lon = (frame_rx->payload[24]) | (frame_rx->payload[25] << 8) |
                   (frame_rx->payload[26] << 16) | (frame_rx->payload[27] << 24),
            .lat = (frame_rx->payload[28]) | (frame_rx->payload[29] << 8) |
                   (frame_rx->payload[30] << 16) | (frame_rx->payload[31] << 24),
            .height = (frame_rx->payload[32]) | (frame_rx->payload[33] << 8) |
                      (frame_rx->payload[34] << 16) | (frame_rx->payload[35] << 24),
            .hMSL = (frame_rx->payload[36]) | (frame_rx->payload[37] << 8) |
                    (frame_rx->payload[38] << 16) | (frame_rx->payload[39] << 24),
            .hAcc = (frame_rx->payload[40]) | (frame_rx->payload[41] << 8) |
                    (frame_rx->payload[42] << 16) | (frame_rx->payload[43] << 24),
            .vAcc = (frame_rx->payload[44]) | (frame_rx->payload[45] << 8) |
                    (frame_rx->payload[46] << 16) | (frame_rx->payload[47] << 24),
            .velN = (frame_rx->payload[48]) | (frame_rx->payload[49] << 8) |
                    (frame_rx->payload[50] << 16) | (frame_rx->payload[51] << 24),
            .velE = (frame_rx->payload[52]) | (frame_rx->payload[53] << 8) |
                    (frame_rx->payload[54] << 16) | (frame_rx->payload[55] << 24),
            .velD = (frame_rx->payload[56]) | (frame_rx->payload[57] << 8) |
                    (frame_rx->payload[58] << 16) | (frame_rx->payload[59] << 24),
            .gSpeed = (frame_rx->payload[60]) | (frame_rx->payload[61] << 8) |
                      (frame_rx->payload[62] << 16) | (frame_rx->payload[63] << 24),
            .headMot = (frame_rx->payload[64]) | (frame_rx->payload[65] << 8) |
                       (frame_rx->payload[66] << 16) | (frame_rx->payload[67] << 24),
            .sAcc = (frame_rx->payload[68]) | (frame_rx->payload[69] << 8) |
                    (frame_rx->payload[70] << 16) | (frame_rx->payload[71] << 24),
            .headAcc = (frame_rx->payload[72]) | (frame_rx->payload[73] << 8) |
                       (frame_rx->payload[74] << 16) | (frame_rx->payload[75] << 24),
            .pDOP = (frame_rx->payload[76]) | (frame_rx->payload[77] << 8),
            .flags3 = (frame_rx->payload[78]) | (frame_rx->payload[79] << 8),
            .reserved1 = frame_rx->payload[80],
            .reserved2 = frame_rx->payload[81],
            .reserved3 = frame_rx->payload[82],
            .reserved4 = frame_rx->payload[83],
            .headVeh = (frame_rx->payload[84]) | (frame_rx->payload[85] << 8) |
                       (frame_rx->payload[86] << 16) | (frame_rx->payload[87] << 24),
            .magDec = (frame_rx->payload[88]) | (frame_rx->payload[89] << 8),
            .magAcc = (frame_rx->payload[90]) | (frame_rx->payload[91] << 8),
        };

        // Using a local variable for nav_pvt is fine, because nav_pvt in
        // the Ublox struct is also not a pointer, so this assignment
        // effectively compiles to a memcpy.
        ublox->nav_pvt = nav_pvt;
        ublox_frame_free(frame_rx);
        return true;
    }
    return false;
}

bool ublox_worker_read_odo(UbloxWorker* ublox_worker) {
    Ublox* ublox = ublox_worker->context;
    UbloxFrame* frame_tx = malloc(sizeof(UbloxFrame));
    frame_tx->class = UBX_NAV_CLASS;
    frame_tx->id = UBX_NAV_ODO_MESSAGE;
    frame_tx->len = 0;
    frame_tx->payload = NULL;
    UbloxMessage* message_tx = ublox_frame_to_bytes(frame_tx);
    ublox_frame_free(frame_tx);

    UbloxMessage* message_rx = ublox_worker_i2c_transfer(message_tx, UBX_NAV_ODO_MESSAGE_LENGTH);
    ublox_message_free(message_tx);
    if(message_rx == NULL) {
        FURI_LOG_E(TAG, "read_odo transfer failed");
        return false;
    }
    UbloxFrame* frame_rx = ublox_bytes_to_frame(message_rx);
    ublox_message_free(message_rx);

    if(frame_rx == NULL) {
        FURI_LOG_E(TAG, "NULL pointer, something wrong with NAV-ODO message!");
        return false;
    } else {
        Ublox_NAV_ODO_Message nav_odo = {
            .version = frame_rx->payload[0],
            .reserved1 = frame_rx->payload[1],
            .reserved2 = frame_rx->payload[2],
            .reserved3 = frame_rx->payload[3],
            .iTOW = (frame_rx->payload[4]) | (frame_rx->payload[5] << 8) |
                    (frame_rx->payload[6] << 16) | (frame_rx->payload[7] << 24),
            .distance = (frame_rx->payload[8]) | (frame_rx->payload[9] << 8) |
                        (frame_rx->payload[10] << 16) | (frame_rx->payload[11] << 24),
            .totalDistance = (frame_rx->payload[12]) | (frame_rx->payload[13] << 8) |
                             (frame_rx->payload[14] << 16) | (frame_rx->payload[15] << 24),
            .distanceStd = (frame_rx->payload[16]) | (frame_rx->payload[17] << 8) |
                           (frame_rx->payload[18] << 16) | (frame_rx->payload[19] << 24),
        };
	FURI_LOG_I(TAG, "odo (m): %lu", nav_odo.distance);
        ublox->nav_odo = nav_odo;
        ublox_frame_free(frame_rx);
        return true;
    }
}

/**
 * Set the power mode to "Balanced", enable the odometer, and
 * configure odometer and dynamic platform model according to user
 * settings.
 */
bool ublox_worker_init_gps(UbloxWorker* ublox_worker) {
    Ublox* ublox = ublox_worker->context;
    // Set power mode
    /*** read initial cfg-pms configuration first ***/
    UbloxFrame pms_frame_tx;
    pms_frame_tx.class = UBX_CFG_CLASS;
    pms_frame_tx.id = UBX_CFG_PMS_MESSAGE;
    pms_frame_tx.len = 0;
    pms_frame_tx.payload = NULL;
    UbloxMessage* pms_message_tx = ublox_frame_to_bytes(&pms_frame_tx);

    UbloxMessage* pms_message_rx =
        ublox_worker_i2c_transfer(pms_message_tx, UBX_CFG_PMS_MESSAGE_LENGTH);
    ublox_message_free(pms_message_tx);
    if(pms_message_rx == NULL) {
        FURI_LOG_E(TAG, "CFG-PMS read transfer failed");
        return false;
    }

    // set power setup value to "balanced"
    pms_message_rx->message[6 + 1] = 0x01;

    pms_frame_tx.class = UBX_CFG_CLASS;
    pms_frame_tx.id = UBX_CFG_PMS_MESSAGE;
    pms_frame_tx.len = 8;
    pms_frame_tx.payload = pms_message_rx->message;

    pms_message_tx = ublox_frame_to_bytes(&pms_frame_tx);

    UbloxMessage* ack = ublox_worker_i2c_transfer(pms_message_tx, UBX_ACK_ACK_MESSAGE_LENGTH);
    if(ack == NULL) {
        FURI_LOG_E(TAG, "ACK after CFG-PMS set transfer failed");
        return false;
    }
    FURI_LOG_I(
        TAG, "CFG-PMS ack: id = %u, type = %s", ack->message[3], ack->message[3] ? "ACK" : "NAK");
    ublox_message_free(pms_message_tx);
    ublox_message_free(pms_message_rx);
    ublox_message_free(ack);

    /***** Odometer *****/
    // Enable odometer by changing CFG-ODO.
    UbloxFrame odo_frame_tx;
    odo_frame_tx.class = UBX_CFG_CLASS;
    odo_frame_tx.id = UBX_CFG_ODO_MESSAGE;
    odo_frame_tx.len = 0;
    odo_frame_tx.payload = NULL;
    UbloxMessage* odo_message_tx = ublox_frame_to_bytes(&odo_frame_tx);

    UbloxMessage* odo_message_rx =
        ublox_worker_i2c_transfer(odo_message_tx, UBX_CFG_ODO_MESSAGE_LENGTH);
    ublox_message_free(odo_message_tx);
    if(odo_message_rx == NULL) {
        FURI_LOG_E(TAG, "CFG-ODO transfer failed");
        return false;
    }

    odo_frame_tx.class = UBX_CFG_CLASS;
    odo_frame_tx.id = UBX_CFG_ODO_MESSAGE;
    odo_frame_tx.len = 20;
    odo_frame_tx.payload = odo_message_rx->message;

    // TODO: low-pass filters in settings?
    // enable useODO bit in flags
    odo_frame_tx.payload[4] |= 1;
    odo_frame_tx.payload[5] = (ublox->device_state).odometer_mode;

    odo_message_tx = ublox_frame_to_bytes(&odo_frame_tx);

    ack = ublox_worker_i2c_transfer(odo_message_tx, UBX_ACK_ACK_MESSAGE_LENGTH);
    if(ack == NULL) {
        FURI_LOG_E(TAG, "ACK after CFG-ODO set transfer failed");
        return false;
    }
    FURI_LOG_I(
        TAG, "CFG-ODO ack: id = %u, type = %s", ack->message[3], ack->message[3] ? "ACK" : "NAK");

    ublox_message_free(odo_message_tx);
    ublox_message_free(odo_message_rx);
    ublox_message_free(ack);

    // finally configure the navigation engine
    UbloxFrame nav5_frame_tx;
    nav5_frame_tx.class = UBX_CFG_CLASS;
    nav5_frame_tx.id = UBX_CFG_NAV5_MESSAGE;
    nav5_frame_tx.len = 0;
    nav5_frame_tx.payload = NULL;
    UbloxMessage* nav5_message_tx = ublox_frame_to_bytes(&nav5_frame_tx);

    UbloxMessage* nav5_message_rx =
        ublox_worker_i2c_transfer(nav5_message_tx, UBX_CFG_NAV5_MESSAGE_LENGTH);
    ublox_message_free(nav5_message_tx);
    
    if(nav5_message_rx == NULL) {
        FURI_LOG_E(TAG, "CFG-NAV5 transfer failed");
        return false;
    }

    // first two bytes tell the GPS what changes to apply, setting this
    // bit tells it to apply the dynamic platfrom model settings.
    nav5_frame_tx.class = UBX_CFG_CLASS;
    nav5_frame_tx.id = UBX_CFG_NAV5_MESSAGE;
    nav5_frame_tx.len = 36;
    nav5_frame_tx.payload = nav5_message_rx->message;

    nav5_frame_tx.payload[0] = 1; // tell GPS to apply only the platform model settings
    nav5_frame_tx.payload[2] = (ublox->device_state).platform_model;

    nav5_message_tx = ublox_frame_to_bytes(&nav5_frame_tx);

    ack = ublox_worker_i2c_transfer(nav5_message_tx, UBX_ACK_ACK_MESSAGE_LENGTH);
    if(ack == NULL) {
        FURI_LOG_E(TAG, "ACK after CFG-NAV5 set transfer failed");
        return false;
    }
    FURI_LOG_I(
        TAG, "CFG-NAV5 ack: id = %u, type = %s", ack->message[3], ack->message[3] ? "ACK" : "NAK");


    ublox_message_free(nav5_message_rx);
    ublox_message_free(ack);
    return true;
}

// this one is being kind of slow
void ublox_worker_reset_odo(UbloxWorker* ublox_worker) {
    FURI_LOG_I(TAG, "ublox_worker_reset_odo");
    UbloxFrame odo_frame_tx;
    odo_frame_tx.class = UBX_NAV_CLASS;
    odo_frame_tx.id = UBX_NAV_RESETODO_MESSAGE;
    odo_frame_tx.len = 0;
    odo_frame_tx.payload = NULL;
    UbloxMessage* odo_message_tx = ublox_frame_to_bytes(&odo_frame_tx);

    UbloxMessage* ack = ublox_worker_i2c_transfer(odo_message_tx, UBX_ACK_ACK_MESSAGE_LENGTH);
    
    ublox_message_free(odo_message_tx);
    
    if(ack == NULL) {
        FURI_LOG_E(TAG, "ACK after NAV-RESETODO set transfer failed");
        ublox_worker->callback(UbloxWorkerEventFailed, ublox_worker->context);
        return;
    } else {
	FURI_LOG_I(
	    TAG,
	    "NAV-RESETODO ack: id = %u, type = %s",
	    ack->message[3],
	    ack->message[3] ? "ACK" : "NAK");
	
	ublox_message_free(ack);
    }
    ublox_worker->callback(UbloxWorkerEventOdoReset, ublox_worker->context);
    // no reason to trigger an event on success, the user will see that
    // the odometer has been reset on the next update.
}