diff --git a/device/bootloader/Source/localization.c b/device/bootloader/Source/localization.c index 6f68eda..906193c 100644 --- a/device/bootloader/Source/localization.c +++ b/device/bootloader/Source/localization.c @@ -8,6 +8,7 @@ typedef struct { db_lh2_t lh2; + uint32_t initialized; double coordinates[2]; } localization_data_t; @@ -16,7 +17,11 @@ static __attribute__((aligned(4))) localization_data_t _localization_data = { 0 void localization_init(void) { puts("Initialize localization"); - db_lh2_init(&_localization_data.lh2, &db_lh2_d, &db_lh2_e); + _localization_data.initialized = db_lh2_init(&_localization_data.lh2, &db_lh2_d, &db_lh2_e); + if (!_localization_data.initialized) { + return; + } + db_lh2_start(); #if LH2_CALIBRATION_IS_VALID @@ -34,12 +39,16 @@ void localization_init(void) { } bool localization_process_data(void) { + if (!LH2_CALIBRATION_IS_VALID || !_localization_data.initialized) { + return false; + } + db_lh2_process_location(&_localization_data.lh2); return (_localization_data.lh2.data_ready[0][0] == DB_LH2_PROCESSED_DATA_AVAILABLE && _localization_data.lh2.data_ready[1][0] == DB_LH2_PROCESSED_DATA_AVAILABLE); } bool localization_get_position(position_2d_t *position) { - if ((LH2_CALIBRATION_IS_VALID) && (_localization_data.lh2.data_ready[0][0] == DB_LH2_PROCESSED_DATA_AVAILABLE && _localization_data.lh2.data_ready[1][0] == DB_LH2_PROCESSED_DATA_AVAILABLE)) { + if ((LH2_CALIBRATION_IS_VALID) && (_localization_data.initialized) && (_localization_data.lh2.data_ready[0][0] == DB_LH2_PROCESSED_DATA_AVAILABLE && _localization_data.lh2.data_ready[1][0] == DB_LH2_PROCESSED_DATA_AVAILABLE)) { db_lh2_stop(); db_lh2_calculate_position(_localization_data.lh2.locations[0][0].lfsr_counts, _localization_data.lh2.locations[1][0].lfsr_counts, _localization_data.lh2.locations[0][0].selected_polynomial, _localization_data.coordinates); _localization_data.lh2.data_ready[0][0] = DB_LH2_NO_NEW_DATA;