fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-21 09:15:23 +05:30 committed by Lorenz Meier
parent 269d05ff22
commit 137ade308f
6 changed files with 99 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cmake/configs/nuttx_px4fmu-v2_default.cmake
View File

@ -98,6 +98,7 @@ set(config_module_list
modules/gpio_led
modules/uavcan
modules/land_detector
modules/tempcal
#
# Estimation modules

17
src/modules/tempcal/matrix_alg.cpp
View File

@ -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);
}

2
src/modules/tempcal/matrix_alg.h
View File

@ -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);

50
src/modules/tempcal/polyfit.cpp
View File

@ -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;

12
src/modules/tempcal/polyfit.h
View File

@ -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);
};

102
src/modules/tempcal/tempcal_main.cpp
View File

@ -53,7 +53,7 @@
#include <poll.h>
#include <time.h>
#include <float.h>
#include <vector>
#include <arch/board/board.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@ -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<math::LowPassFilter2p> _lp_roll_rate;
std::vector<math::LowPassFilter2p> _lp_pitch_rate;
std::vector<math::LowPassFilter2p> _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++) {