fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 12:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

tempcal: change to double as float was insufficient for calculation

Browse Source
This commit is contained in:
Siddharth Bharat Purohit
2017-01-25 22:43:29 +01:00
committed by Lorenz Meier
parent 269d05ff22
commit 137ade308f
6 changed files with 99 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cmake/configs/nuttx_px4fmu-v2_default.cmake
+1
View File
@@ -98,6 +98,7 @@ set(config_module_list
modules/gpio_led modules/gpio_led
modules/uavcan modules/uavcan
modules/land_detector modules/land_detector
modules/tempcal
# #
# Estimation modules # Estimation modules
src/modules/tempcal/matrix_alg.cpp
+4 -13
View File
@@ -256,9 +256,9 @@ bool mat_inverse(float *A, float *inv, uint8_t n)
return ret; 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; uint8_t i;
inv[0] = m[5] * m[10] * m[15] - inv[0] = m[5] * m[10] * m[15] -
@@ -379,7 +379,7 @@ bool inverse4x4(float m[], float invOut[])
return false; return false;
} }
det = 1.0f / det; det = 1.0 / det;
for (i = 0; i < 16; i++) { for (i = 0; i < 16; i++) {
invOut[i] = inv[i] * det; invOut[i] = inv[i] * det;
@@ -390,14 +390,5 @@ bool inverse4x4(float m[], float invOut[])
bool inverse(float *A, float *inv, uint8_t n) bool inverse(float *A, float *inv, uint8_t n)
{ {
printf("Got Here!!\n"); return mat_inverse(A, inv, n);
if (n == 4) {
printf("Got Here4!!\n");
return inverse4x4(A, inv);
} else {
return mat_inverse(A, inv, n);
}
} }
src/modules/tempcal/matrix_alg.h
+1 -1
View File
@@ -47,5 +47,5 @@
float *mat_mul(float *A, float *B, uint8_t n); float *mat_mul(float *A, float *B, uint8_t n);
bool mat_inverse(float *A, float *inv, 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); bool inverse(float *A, float *inv, uint8_t n);
src/modules/tempcal/polyfit.cpp
+36 -14
View File
@@ -40,45 +40,57 @@ Author: Siddharth Bharat Purohit
int polyfitter::init(uint8_t order) int polyfitter::init(uint8_t order)
{ {
_forder = order + 1; _forder = order + 1;
VTV = new float[_forder * _forder]; VTV = new double[_forder * _forder];
if (VTV == NULL) { if (VTV == NULL) {
return -1; return -1;
} }
VTY = new float[_forder]; VTY = new double[_forder];
if (VTY == NULL) { if (VTY == NULL) {
return -1; return -1;
} }
memset(VTV, 0, sizeof(float)*_forder * _forder); memset(VTV, 0, sizeof(double)*_forder * _forder);
memset(VTY, 0, sizeof(float)*_forder); memset(VTY, 0, sizeof(double)*_forder);
return 0; return 0;
} }
void polyfitter::update(float x, float y) void polyfitter::update(double x, double y)
{ {
update_VTV(x); update_VTV(x);
update_VTY(x, y); 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--) { for (int8_t i = _forder - 1; i >= 0; i--) {
VTY[i] += y * temp; VTY[i] += y * temp;
temp *= x; 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; 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--) { for (int8_t i = 2 * _forder - 2; i >= 0; i--) {
if (i < _forder) { if (i < _forder) {
z = 0.0f; z = 0.0f;
@@ -90,29 +102,39 @@ void polyfitter::update_VTV(float x)
for (int8_t j = i - z; j >= z; j--) { for (int8_t j = i - z; j >= z; j--) {
uint8_t row = j; uint8_t row = j;
uint8_t col = i - j; uint8_t col = i - j;
VTV[row * _forder + col] += temp; VTV[row * _forder + col] += (double)temp;
} }
temp *= x; temp *= x;
} }
} }
bool polyfitter::fit(float res[]) bool polyfitter::fit(double res[])
{ {
//Do inverse of VTV //Do inverse of VTV
float *IVTV = new float[_forder * _forder]; double *IVTV = new double[_forder * _forder];
if (VTV == NULL) { if (VTV == NULL) {
return false; 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++) { for (uint8_t i = 0; i < _forder; i++) {
res[i] = 0.0f; res[i] = 0.0f;
for (int j = 0; j < _forder; j++) { 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; return true;
src/modules/tempcal/polyfit.h
+6 -6
View File
@@ -65,13 +65,13 @@ public:
delete[] VTY; delete[] VTY;
} }
} }
void update(float x, float y); void update(double x, double y);
int init(uint8_t order); int init(uint8_t order);
bool fit(float res[]); bool fit(double res[]);
private: private:
float *VTV; double *VTV;
float *VTY; double *VTY;
uint8_t _forder; uint8_t _forder;
void update_VTV(float x); void update_VTV(double x);
void update_VTY(float x, float y); void update_VTY(double x, double y);
}; };
src/modules/tempcal/tempcal_main.cpp
+51 -51
View File
@@ -53,7 +53,7 @@
#include <poll.h> #include <poll.h>
#include <time.h> #include <time.h>
#include <float.h> #include <float.h>
#include <vector>
#include <arch/board/board.h> #include <arch/board/board.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <systemlib/err.h> #include <systemlib/err.h>
@@ -113,18 +113,17 @@ private:
int _control_task = -1; // task handle for task int _control_task = -1; // task handle for task
/* Low pass filter for attitude rates */ /* Low pass filter for attitude rates */
math::LowPassFilter2p _lp_roll_rate; std::vector<math::LowPassFilter2p> _lp_roll_rate;
math::LowPassFilter2p _lp_pitch_rate; std::vector<math::LowPassFilter2p> _lp_pitch_rate;
math::LowPassFilter2p _lp_yaw_rate; std::vector<math::LowPassFilter2p> _lp_yaw_rate;
}; };
Tempcal::Tempcal(): Tempcal::Tempcal():
SuperBlock(NULL, "EKF"), SuperBlock(NULL, "Tempcal"),
_lp_roll_rate(250.0f, 1.0f), _lp_roll_rate(SENSOR_COUNT_MAX, math::LowPassFilter2p(250.0f, 1.0f)),
_lp_pitch_rate(250.0f, 1.0f), _lp_pitch_rate(SENSOR_COUNT_MAX, math::LowPassFilter2p(250.0f, 1.0f)),
_lp_yaw_rate(250.0f, 1.0f) _lp_yaw_rate(SENSOR_COUNT_MAX, math::LowPassFilter2p(250.0f, 1.0f))
{ {
} }
Tempcal::~Tempcal() Tempcal::~Tempcal()
@@ -150,8 +149,9 @@ void Tempcal::task_main()
bool _cold_soaked[SENSOR_COUNT_MAX] = {false}; bool _cold_soaked[SENSOR_COUNT_MAX] = {false};
bool _hot_soaked[SENSOR_COUNT_MAX] = {false}; bool _hot_soaked[SENSOR_COUNT_MAX] = {false};
bool _tempcal_complete[SENSOR_COUNT_MAX] = {false};
float _low_temp[SENSOR_COUNT_MAX], _high_temp[SENSOR_COUNT_MAX]; float _low_temp[SENSOR_COUNT_MAX];
float _high_temp[SENSOR_COUNT_MAX] = {0};
for (unsigned i = 0; i < num_gyro; i++) { for (unsigned i = 0; i < num_gyro; i++) {
if (gyro_sub[i] < 0) { if (gyro_sub[i] < 0) {
@@ -171,6 +171,7 @@ void Tempcal::task_main()
// because they will else not always be // because they will else not always be
// properly populated // properly populated
sensor_gyro_s gyro_data = {}; sensor_gyro_s gyro_data = {};
//uint16_t l = 0;
while (!_task_should_exit) { while (!_task_should_exit) {
int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
@@ -192,9 +193,10 @@ void Tempcal::task_main()
if (fds[i].revents & POLLIN) { if (fds[i].revents & POLLIN) {
orb_copy(ORB_ID(sensor_gyro), gyro_sub[i], &gyro_data); 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][0] = _lp_roll_rate[i].apply(gyro_data.x);
gyro_sample_filt[i][2] = _lp_roll_rate.apply(gyro_data.z); 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; gyro_sample_filt[i][3] = gyro_data.temperature;
if (!_cold_soaked[i]) { 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]) { if (_hot_soaked[i]) {
continue; 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; continue;
} }
if (gyro_sample_filt[i][3] <= _low_temp[i]) { if (_hot_soak_sat[i] == 10 || (_high_temp[i] - _low_temp[i]) > 24.0f) {
//We are not hot soaking increment hot soak saturation count
_hot_soak_sat[i]++;
}
if (_hot_soak_sat[i] == 10) {
_hot_soaked[i] = true; _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], if (i == 0) {
(double)gyro_sample_filt[i][2], (double)gyro_sample_filt[i][3]); 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 //update linear fit matrices
P[i][0].update(gyro_sample_filt[i][0], 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(gyro_sample_filt[i][1], gyro_sample_filt[i][3]); P[i][1].update((double)gyro_sample_filt[i][3], (double)gyro_sample_filt[i][1]);
P[i][2].update(gyro_sample_filt[i][2], gyro_sample_filt[i][3]); P[i][2].update((double)gyro_sample_filt[i][3], (double)gyro_sample_filt[i][2]);
num_samples[i] = 0; num_samples[i] = 0;
} }
if (_collection_complete) { for (uint8_t i = 0; i < 1; i++) {
for (uint8_t i = 0; i < num_gyro; i++) { if (_hot_soaked[i] && !_tempcal_complete[i]) {
if (_high_temp[i] - _low_temp[i] > 20) { double res[4];
PX4_WARN("Cal Failed for Gyro %d", i); 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],
} else { (double)res[3]);
float res[4]; P[i][1].fit(res);
P[i][0].fit(res); PX4_WARN("Result Gyro %d Axis 1: %.20f %.20f %.20f %.20f", i, (double)res[0], (double)res[1], (double)res[2],
PX4_WARN("Result Gyro %d Axis 0: %.6f %.6f %.6f %.6f", i, (double)res[0], (double)res[1], (double)res[2], (double)res[3]);
(double)res[3]); P[i][2].fit(res);
P[i][1].fit(res); PX4_WARN("Result Gyro %d Axis 2: %.20f %.20f %.20f %.20f", i, (double)res[0], (double)res[1], (double)res[2],
PX4_WARN("Result Gyro %d Axis 1: %.6f %.6f %.6f %.6f", i, (double)res[0], (double)res[1], (double)res[2], (double)res[3]);
(double)res[3]); _tempcal_complete[i] = true;
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]);
}
} }
break; //complete temp cal
} }
usleep(100);
} }
for (uint8_t i = 0; i < num_gyro; i++) { for (uint8_t i = 0; i < num_gyro; i++) {