diff --git a/fs_helper.h b/fs_helper.h new file mode 100644 index 0000000..597861e --- /dev/null +++ b/fs_helper.h @@ -0,0 +1,46 @@ +#include +#include + +#include "FS.h" + +mutex_t littelfs_mut; + +void init_little_fs() { + mutex_init(&littelfs_mut); + mutex_enter_blocking(&littelfs_mut); + if (!LittleFS.begin()) { + LittleFS.format(); + LittleFS.begin(); + } + mutex_exit(&littelfs_mut); +} + +bool fs_read(String path, void *buffer, size_t len) { + char centerData[len]; + mutex_enter_blocking(&littelfs_mut); + File f = LittleFS.open(path, "r"); + bool succ = false; + if (f) { + if (f.readBytes((char *)centerData, len) == len) { + memmove(&buffer, ¢erData, len); + succ = true; + } + f.close(); + } + mutex_exit(&littelfs_mut); + return succ; +} + +bool fs_write(String path, void *buffer, size_t len) { + mutex_enter_blocking(&littelfs_mut); + File f = LittleFS.open(path, "w"); + bool succ = false; + if (f) { + if (f.write((const unsigned char *)buffer, len) == len) { + succ = true; + } + f.close(); + } + mutex_exit(&littelfs_mut); + return succ; +} \ No newline at end of file diff --git a/juRCReceiver.h b/juRCReceiver.h index 334853c..71707ad 100644 --- a/juRCReceiver.h +++ b/juRCReceiver.h @@ -16,12 +16,14 @@ #define debugReceive false #endif -#include +// #include #include #include #include #include +#include "fs_helper.h" + // #include "structsAndDefines.h" @@ -37,8 +39,6 @@ void jSetOnData(on_data conn) { _j_on_data = conn; } // 3 (CE) 0 //4 (CSN) 4 //5 (SCK) 18 //6 (MOSI) 23 //7 (MISO) 19 // CE CSN -Preferences preferences; - unsigned long last = 0; unsigned long delta = 0; float avg = 10; @@ -57,13 +57,7 @@ void jSetup() { #if debug Serial.println("start up"); #endif -#if debug - bool prefsucc = -#endif - preferences.begin("data", false); -#if debug - if (!prefsucc) Serial.println("prefs false"); -#endif + initRC(); #if debug Serial.println("inited rc"); @@ -89,7 +83,7 @@ void jSetup() { last = millis(); } void jSensorLoop() { - unsigned long now = millis(); + unsigned long now = micros(); delta = now - last; last = now; avg = avg * 0.9 + delta * 0.1; diff --git a/system_sensors/compass.h b/system_sensors/compass.h index 88603e7..ead6eca 100644 --- a/system_sensors/compass.h +++ b/system_sensors/compass.h @@ -17,6 +17,12 @@ typedef struct { #ifdef jCOMP +typedef struct { + float x; + float y; + float z; +} compassPrefData; + #include QMC5883LCompass compass; @@ -24,12 +30,21 @@ bool compass_is_calibrating = false; void initCOMP() { compass.init(); - compass.setCalibrationOffsets(preferences.getFloat("compOffX", 0), - preferences.getFloat("compOffY", 0), - preferences.getFloat("compOffZ", 0)); - compass.setCalibrationScales(preferences.getFloat("compScaleX", 1), - preferences.getFloat("compScaleY", 1), - preferences.getFloat("compScaleZ", 1)); + + compassPrefData buffer; + + if (!fs_read("compOff", &buffer, sizeof(buffer))) { + buffer.x = 0; + buffer.y = 0; + buffer.z = 0; + } + compass.setCalibrationOffsets(buffer.x, buffer.y, buffer.z); + if (!fs_read("compScale", &buffer, sizeof(buffer))) { + buffer.x = 1; + buffer.y = 1; + buffer.z = 1; + } + compass.setCalibrationScales(buffer.x, buffer.y, buffer.z); } void COMPLoop() { @@ -58,13 +73,15 @@ void COMPradioTask() { delay(10); compass.calibrate(); - preferences.putFloat("compOffX", compass.getCalibrationOffset(0)); - preferences.putFloat("compOffY", compass.getCalibrationOffset(1)); - preferences.putFloat("compOffZ", compass.getCalibrationOffset(2)); - - preferences.putFloat("compScaleX", compass.getCalibrationScale(0)); - preferences.putFloat("compScaleY", compass.getCalibrationScale(1)); - preferences.putFloat("compScaleZ", compass.getCalibrationScale(2)); + compassPrefData buffer; + buffer.x = compass.getCalibrationOffset(0); + buffer.y = compass.getCalibrationOffset(1); + buffer.z = compass.getCalibrationOffset(2); + fs_write("compOff", &buffer, sizeof(buffer)); + buffer.x = compass.getCalibrationScale(0); + buffer.y = compass.getCalibrationScale(1); + buffer.z = compass.getCalibrationScale(2); + fs_write("compScale", &buffer, sizeof(buffer)); compass_is_calibrating = false; } break; } diff --git a/system_sensors/mpu.h b/system_sensors/mpu.h index fd1f9eb..4b07e18 100644 --- a/system_sensors/mpu.h +++ b/system_sensors/mpu.h @@ -32,6 +32,12 @@ typedef struct { #ifdef jMPU6050 +typedef struct { + float x; + float y; + float z; +} mpuPrefData; + #include MPU6050 mpu6050(Wire, 0.001f, 0.999f); @@ -42,12 +48,22 @@ void initMPU() { Serial.println("began mpu6050"); #endif - mpu6050.setGyroOffsets(preferences.getFloat("mpuOffGyrX", -7.8), - preferences.getFloat("mpuOffGyrY", 2.32), - preferences.getFloat("mpuOffGyrZ", -0.22)); - mpu6050.setAccXoffset(preferences.getFloat("mpuOffAccX", 0.03), - preferences.getFloat("mpuOffAccY", 0.0), - preferences.getFloat("mpuOffAccZ", 0.3)); + mpuPrefData buffer; + + if (!fs_read("mpuOffGyr", &buffer, sizeof(buffer))) { + buffer.x = -7.8; + buffer.y = 2.32; + buffer.z = -0.22; + } + mpu6050.setGyroOffsets(buffer.x, buffer.y, buffer.z); + + if (!fs_read("mpuOffAcc", &buffer, sizeof(buffer))) { + buffer.x = 0.03; + buffer.y = 0.0; + buffer.z = 0.3; + } + mpu6050.setAccXoffset(buffer.x, buffer.y, buffer.z); + #if debug Serial.println("set mpu6050 gyro"); #endif @@ -128,18 +144,16 @@ void MPUradioTask() { float offX = mpu6050.calcAccXoffset(); float offY = mpu6050.calcAccYoffset(); float offZ = mpu6050.calcAccZoffset(); - preferences.putFloat("mpuOffAccX", offX); - preferences.putFloat("mpuOffAccY", offY); - preferences.putFloat("mpuOffAccZ", offZ); + mpuPrefData buffer{.x = offX, .y = offY, .z = offZ}; + fs_write("mpuOffAcc", &buffer, sizeof(buffer)); mpu6050.setAccXoffset(offX, offY, offZ); } break; case jCommand_MPU_set_calc_offsets_gyro: { float offX = mpu6050.getGyroXoffsetGlide(); float offY = mpu6050.getGyroYoffsetGlide(); float offZ = mpu6050.getGyroZoffsetGlide(); - preferences.putFloat("mpuOffGyrX", offX); - preferences.putFloat("mpuOffGyrY", offY); - preferences.putFloat("mpuOffGyrZ", offZ); + mpuPrefData buffer{.x = offX, .y = offY, .z = offZ}; + fs_write("mpuOffGyr", &buffer, sizeof(buffer)); mpu6050.setGyroOffsets(offX, offY, offZ); } break; case jCommand_MPU_get_calc_offsets_acc: { @@ -161,34 +175,46 @@ void MPUradioTask() { case jCommand_MPU_set_offsets_acc: { mpuSendData data; memmove(&data, &jRCbuff, sizeof(data)); - preferences.putFloat("mpuOffGyrX", data.X); - preferences.putFloat("mpuOffGyrY", data.Y); - preferences.putFloat("mpuOffGyrZ", data.Z); + mpuPrefData buffer{.x = data.X, .y = data.Y, .z = data.Z}; + fs_write("mpuOffAcc", &buffer, sizeof(buffer)); mpu6050.setGyroOffsets(data.X, data.Y, data.Z); } break; case jCommand_MPU_set_offsets_gyro: { mpuSendData data; memmove(&data, &jRCbuff, sizeof(data)); - preferences.putFloat("mpuOffAccX", data.X); - preferences.putFloat("mpuOffAccY", data.Y); - preferences.putFloat("mpuOffAccZ", data.Z); + mpuPrefData buffer{.x = data.X, .y = data.Y, .z = data.Z}; + fs_write("mpuOffGyr", &buffer, sizeof(buffer)); mpu6050.setAccXoffset(data.X, data.Y, data.Z); } break; case jCommand_MPU_get_offsets_acc: { mpuSendData data; data.command = jCommand_MPU_get_offsets_acc; - data.X = preferences.getFloat("mpuOffGyrX", -7.8); - data.Y = preferences.getFloat("mpuOffGyrY", 2.32); - data.Z = preferences.getFloat("mpuOffGyrZ", -0.22); + + mpuPrefData buffer; + if (!fs_read("mpuOffAcc", &buffer, sizeof(buffer))) { + buffer.x = 0.03; + buffer.y = 0.0; + buffer.z = 0.3; + } + data.X = buffer.x; + data.Y = buffer.y; + data.Z = buffer.z; jSendNRF(&data, sizeof(data)); } break; case jCommand_MPU_get_offsets_gyro: { mpuSendData data; data.command = jCommand_MPU_get_offsets_gyro; - data.X = preferences.getFloat("mpuOffAccX", 0.03); - data.Y = preferences.getFloat("mpuOffAccY", 0.0); - data.Z = preferences.getFloat("mpuOffAccZ", 0.3); + + mpuPrefData buffer; + if (!fs_read("mpuOffGyr", &buffer, sizeof(buffer))) { + buffer.x = -7.8; + buffer.y = 2.32; + buffer.z = -0.22; + } + data.X = buffer.x; + data.Y = buffer.y; + data.Z = buffer.z; jSendNRF(&data, sizeof(data)); } }