diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 3ec2d37cfa..f4f1d4187c 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -98,6 +98,7 @@ set(config_module_list modules/gpio_led modules/uavcan modules/land_detector + modules/tempcal # # Estimation modules diff --git a/src/modules/tempcal/matrix_alg.cpp b/src/modules/tempcal/matrix_alg.cpp index 99cf4b9989..18a6dfbfb5 100644 --- a/src/modules/tempcal/matrix_alg.cpp +++ b/src/modules/tempcal/matrix_alg.cpp @@ -256,9 +256,9 @@ bool mat_inverse(float *A, float *inv, uint8_t n) return ret; } -bool inverse4x4(float m[], float invOut[]) +bool inverse4x4(double m[], double invOut[]) { - float inv[16], det; + double inv[16], det; uint8_t i; inv[0] = m[5] * m[10] * m[15] - @@ -379,7 +379,7 @@ bool inverse4x4(float m[], float invOut[]) return false; } - det = 1.0f / det; + det = 1.0 / det; for (i = 0; i < 16; i++) { invOut[i] = inv[i] * det; @@ -390,14 +390,5 @@ bool inverse4x4(float m[], float invOut[]) bool inverse(float *A, float *inv, uint8_t n) { - printf("Got Here!!\n"); - - if (n == 4) { - printf("Got Here4!!\n"); - return inverse4x4(A, inv); - - } else { - return mat_inverse(A, inv, n); - } - + return mat_inverse(A, inv, n); } \ No newline at end of file diff --git a/src/modules/tempcal/matrix_alg.h b/src/modules/tempcal/matrix_alg.h index b8b29bfb7c..ed62a55927 100644 --- a/src/modules/tempcal/matrix_alg.h +++ b/src/modules/tempcal/matrix_alg.h @@ -47,5 +47,5 @@ float *mat_mul(float *A, float *B, uint8_t n); bool mat_inverse(float *A, float *inv, uint8_t n); -bool inverse4x4(float m[], float invOut[]); +bool inverse4x4(double m[], double invOut[]); bool inverse(float *A, float *inv, uint8_t n); diff --git a/src/modules/tempcal/polyfit.cpp b/src/modules/tempcal/polyfit.cpp index ebca6a6dd3..8ec3c878ef 100644 --- a/src/modules/tempcal/polyfit.cpp +++ b/src/modules/tempcal/polyfit.cpp @@ -40,45 +40,57 @@ Author: Siddharth Bharat Purohit int polyfitter::init(uint8_t order) { _forder = order + 1; - VTV = new float[_forder * _forder]; + VTV = new double[_forder * _forder]; if (VTV == NULL) { return -1; } - VTY = new float[_forder]; + VTY = new double[_forder]; if (VTY == NULL) { return -1; } - memset(VTV, 0, sizeof(float)*_forder * _forder); - memset(VTY, 0, sizeof(float)*_forder); + memset(VTV, 0, sizeof(double)*_forder * _forder); + memset(VTY, 0, sizeof(double)*_forder); return 0; } -void polyfitter::update(float x, float y) +void polyfitter::update(double x, double y) { update_VTV(x); update_VTY(x, y); } -void polyfitter::update_VTY(float x, float y) +void polyfitter::update_VTY(double x, double y) { - float temp = 1.0f; + double temp = 1.0f; + printf("O %.6f\n", (double)x); for (int8_t i = _forder - 1; i >= 0; i--) { VTY[i] += y * temp; temp *= x; + printf("%.6f ", (double)VTY[i]); } + + printf("\n"); } -void polyfitter::update_VTV(float x) +void polyfitter::update_VTV(double x) { - float temp = 1.0f; + double temp = 1.0f; int8_t z; + for (uint8_t i = 0; i < _forder; i++) { + for (int j = 0; j < _forder; j++) { + printf("%.10f ", (double)VTV[i * _forder + j]); + } + + printf("\n"); + } + for (int8_t i = 2 * _forder - 2; i >= 0; i--) { if (i < _forder) { z = 0.0f; @@ -90,29 +102,39 @@ void polyfitter::update_VTV(float x) for (int8_t j = i - z; j >= z; j--) { uint8_t row = j; uint8_t col = i - j; - VTV[row * _forder + col] += temp; + VTV[row * _forder + col] += (double)temp; } temp *= x; } } -bool polyfitter::fit(float res[]) +bool polyfitter::fit(double res[]) { //Do inverse of VTV - float *IVTV = new float[_forder * _forder]; + double *IVTV = new double[_forder * _forder]; if (VTV == NULL) { return false; } - if (inverse(VTV, IVTV, _forder)) { + if (inverse4x4(VTV, IVTV)) { + for (uint8_t i = 0; i < _forder; i++) { + for (int j = 0; j < _forder; j++) { + printf("%.10f ", (double)IVTV[i * _forder + j]); + } + + printf("\n"); + } + for (uint8_t i = 0; i < _forder; i++) { res[i] = 0.0f; for (int j = 0; j < _forder; j++) { - res[i] += IVTV[i * _forder + j] * VTY[j]; + res[i] += IVTV[i * _forder + j] * (double)VTY[j]; } + + printf("%.10f ", res[i]); } return true; diff --git a/src/modules/tempcal/polyfit.h b/src/modules/tempcal/polyfit.h index 33762099b1..87dfb00413 100644 --- a/src/modules/tempcal/polyfit.h +++ b/src/modules/tempcal/polyfit.h @@ -65,13 +65,13 @@ public: delete[] VTY; } } - void update(float x, float y); + void update(double x, double y); int init(uint8_t order); - bool fit(float res[]); + bool fit(double res[]); private: - float *VTV; - float *VTY; + double *VTV; + double *VTY; uint8_t _forder; - void update_VTV(float x); - void update_VTY(float x, float y); + void update_VTV(double x); + void update_VTY(double x, double y); }; \ No newline at end of file diff --git a/src/modules/tempcal/tempcal_main.cpp b/src/modules/tempcal/tempcal_main.cpp index ceb361c29d..00c0d72f67 100644 --- a/src/modules/tempcal/tempcal_main.cpp +++ b/src/modules/tempcal/tempcal_main.cpp @@ -53,7 +53,7 @@ #include #include #include - +#include #include #include #include @@ -113,18 +113,17 @@ private: int _control_task = -1; // task handle for task /* Low pass filter for attitude rates */ - math::LowPassFilter2p _lp_roll_rate; - math::LowPassFilter2p _lp_pitch_rate; - math::LowPassFilter2p _lp_yaw_rate; + std::vector _lp_roll_rate; + std::vector _lp_pitch_rate; + std::vector _lp_yaw_rate; }; Tempcal::Tempcal(): - SuperBlock(NULL, "EKF"), - _lp_roll_rate(250.0f, 1.0f), - _lp_pitch_rate(250.0f, 1.0f), - _lp_yaw_rate(250.0f, 1.0f) + SuperBlock(NULL, "Tempcal"), + _lp_roll_rate(SENSOR_COUNT_MAX, math::LowPassFilter2p(250.0f, 1.0f)), + _lp_pitch_rate(SENSOR_COUNT_MAX, math::LowPassFilter2p(250.0f, 1.0f)), + _lp_yaw_rate(SENSOR_COUNT_MAX, math::LowPassFilter2p(250.0f, 1.0f)) { - } Tempcal::~Tempcal() @@ -150,8 +149,9 @@ void Tempcal::task_main() bool _cold_soaked[SENSOR_COUNT_MAX] = {false}; bool _hot_soaked[SENSOR_COUNT_MAX] = {false}; - - float _low_temp[SENSOR_COUNT_MAX], _high_temp[SENSOR_COUNT_MAX]; + bool _tempcal_complete[SENSOR_COUNT_MAX] = {false}; + float _low_temp[SENSOR_COUNT_MAX]; + float _high_temp[SENSOR_COUNT_MAX] = {0}; for (unsigned i = 0; i < num_gyro; i++) { if (gyro_sub[i] < 0) { @@ -171,6 +171,7 @@ void Tempcal::task_main() // because they will else not always be // properly populated sensor_gyro_s gyro_data = {}; + //uint16_t l = 0; while (!_task_should_exit) { int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); @@ -192,9 +193,10 @@ void Tempcal::task_main() if (fds[i].revents & POLLIN) { orb_copy(ORB_ID(sensor_gyro), gyro_sub[i], &gyro_data); - gyro_sample_filt[i][0] = _lp_roll_rate.apply(gyro_data.x); - gyro_sample_filt[i][1] = _lp_roll_rate.apply(gyro_data.y); - gyro_sample_filt[i][2] = _lp_roll_rate.apply(gyro_data.z); + + gyro_sample_filt[i][0] = _lp_roll_rate[i].apply(gyro_data.x); + gyro_sample_filt[i][1] = _lp_pitch_rate[i].apply(gyro_data.y); + gyro_sample_filt[i][2] = _lp_yaw_rate[i].apply(gyro_data.z); gyro_sample_filt[i][3] = gyro_data.temperature; if (!_cold_soaked[i]) { @@ -206,60 +208,58 @@ void Tempcal::task_main() } } - bool _collection_complete = true; - for (uint8_t i = 0; i < num_gyro; i++) { + for (uint8_t i = 0; i < 1; i++) { if (_hot_soaked[i]) { continue; - - } else { - _collection_complete = false; } - if (num_samples[i] < 250) { + //if (num_samples[i] < 250) { + // continue; + //} + + if (gyro_sample_filt[i][3] > _high_temp[i]) { + _high_temp[i] = gyro_sample_filt[i][3]; + _hot_soak_sat[i] = 0; + + } else { continue; } - if (gyro_sample_filt[i][3] <= _low_temp[i]) { - //We are not hot soaking increment hot soak saturation count - _hot_soak_sat[i]++; - } - - if (_hot_soak_sat[i] == 10) { + if (_hot_soak_sat[i] == 10 || (_high_temp[i] - _low_temp[i]) > 24.0f) { _hot_soaked[i] = true; - _high_temp[i] = gyro_sample_filt[i][3]; } - printf("Got Here!! %.6f %.6f %.6f %.6f\n", (double)gyro_sample_filt[i][0], (double)gyro_sample_filt[i][1], - (double)gyro_sample_filt[i][2], (double)gyro_sample_filt[i][3]); + if (i == 0) { + printf("%.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n", (double)gyro_sample_filt[i][0], (double)gyro_sample_filt[i][1], + (double)gyro_sample_filt[i][2], (double)gyro_sample_filt[i][3], (double)_low_temp[i], (double)_high_temp[i], + (double)(_high_temp[i] - _low_temp[i])); + } + //update linear fit matrices - P[i][0].update(gyro_sample_filt[i][0], gyro_sample_filt[i][3]); - P[i][1].update(gyro_sample_filt[i][1], gyro_sample_filt[i][3]); - P[i][2].update(gyro_sample_filt[i][2], gyro_sample_filt[i][3]); + P[i][0].update((double)gyro_sample_filt[i][3], (double)gyro_sample_filt[i][0]); + P[i][1].update((double)gyro_sample_filt[i][3], (double)gyro_sample_filt[i][1]); + P[i][2].update((double)gyro_sample_filt[i][3], (double)gyro_sample_filt[i][2]); num_samples[i] = 0; } - if (_collection_complete) { - for (uint8_t i = 0; i < num_gyro; i++) { - if (_high_temp[i] - _low_temp[i] > 20) { - PX4_WARN("Cal Failed for Gyro %d", i); - - } else { - float res[4]; - P[i][0].fit(res); - PX4_WARN("Result Gyro %d Axis 0: %.6f %.6f %.6f %.6f", i, (double)res[0], (double)res[1], (double)res[2], - (double)res[3]); - P[i][1].fit(res); - PX4_WARN("Result Gyro %d Axis 1: %.6f %.6f %.6f %.6f", i, (double)res[0], (double)res[1], (double)res[2], - (double)res[3]); - P[i][2].fit(res); - PX4_WARN("Result Gyro %d Axis 2: %.6f %.6f %.6f %.6f", i, (double)res[0], (double)res[1], (double)res[2], - (double)res[3]); - } + for (uint8_t i = 0; i < 1; i++) { + if (_hot_soaked[i] && !_tempcal_complete[i]) { + double res[4]; + P[i][0].fit(res); + PX4_WARN("Result Gyro %d Axis 0: %.20f %.20f %.20f %.20f", i, (double)res[0], (double)res[1], (double)res[2], + (double)res[3]); + P[i][1].fit(res); + PX4_WARN("Result Gyro %d Axis 1: %.20f %.20f %.20f %.20f", i, (double)res[0], (double)res[1], (double)res[2], + (double)res[3]); + P[i][2].fit(res); + PX4_WARN("Result Gyro %d Axis 2: %.20f %.20f %.20f %.20f", i, (double)res[0], (double)res[1], (double)res[2], + (double)res[3]); + _tempcal_complete[i] = true; } - - break; //complete temp cal } + + usleep(100); } for (uint8_t i = 0; i < num_gyro; i++) {