Skip to content

Commit

Permalink
Merge pull request #328 from hjr/master
Browse files Browse the repository at this point in the history 
Preparation for a CAN MagSens revamp.
  • Loading branch information
@iltis42
iltis42 authored Nov 3, 2024
2 parents 16d5815 + 028ed9a commit bfc3f37
Show file tree
Hide file tree
Showing 13 changed files with 284 additions and 284 deletions.
157 changes: 77 additions & 80 deletions main/Compass.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,17 +16,19 @@ Author: Axel Pauli, deviation and refactoring by Eckhard Völlm Dec 2021
**************************************************************************/

#include <cassert>
#include <cmath>
#include "esp_log.h"
#include "esp_system.h"
#include "sensor.h"
#include "Compass.h"
#include "KalmanMPU6050.h"
#include "QMCMagCAN.h"
#include "QMC5883L.h"
#include "quaternion.h"

#include "esp_log.h"
#include "esp_system.h"
#include "sensor.h"

#include <cassert>
#include <cmath>

TaskHandle_t ctid = 0;

/*
Expand All @@ -52,7 +54,6 @@ Compass::Compass( const uint8_t addr, const uint8_t odr, const uint8_t range, co
_heading_average = -1000;
calibrationRunning = false;
_heading = 0;
holddown = false;
errors = 0;
totalReadErrors = 0;
bias = { 0,0,0 };
Expand All @@ -61,7 +62,7 @@ Compass::Compass( const uint8_t addr, const uint8_t odr, const uint8_t range, co
fx=0;
fy=0;
fz=0;
i=0;
nrsamples=0;
}

Compass::~Compass()
Expand Down Expand Up @@ -212,11 +213,11 @@ void Compass::setHeading( float h ) {

// calibration calculation in sync with data received in compass task
void Compass::calcCalibration(){
i++;
nrsamples++;
float variance = 0.f;
t_float_axes var;
if( sensor ){
if( sensor->rawAxes( axes ) == false )
if( sensor->readRaw( magRaw ) == false )
{
errors++;
if( errors > 100 ){
Expand All @@ -226,40 +227,40 @@ void Compass::calcCalibration(){
return;
}
}
raw.x = (*avgX)( axes.x );
raw.y = (*avgY)( axes.y );
raw.z = (*avgZ)( axes.z );
avg_calib_sample.x = (*avgX)( magRaw.x );
avg_calib_sample.y = (*avgY)( magRaw.y );
avg_calib_sample.z = (*avgZ)( magRaw.z );
// Variance low pass filtered
var.x = axes.x - raw.x;
var.y = axes.y - raw.y;
var.z = axes.z - raw.z;
var.x = magRaw.x - avg_calib_sample.x;
var.y = magRaw.y - avg_calib_sample.y;
var.z = magRaw.z - avg_calib_sample.z;
variance += ((var.x*var.x + var.y*var.y + var.z*var.z) - variance) / 50.f;
// ESP_LOGI( FNAME, "Mag Var: %7.3f", variance );

// ESP_LOGI( FNAME, "Mag Var X:%.2f Y:%.2f Z:%.2f", var.x, var.y, var.z );
/* Find max/min peak values */
min.x = ( raw.x < min.x ) ? raw.x : min.x;
min.y = ( raw.y < min.y ) ? raw.y : min.y;
min.z = ( raw.z < min.z ) ? raw.z : min.z;
max.x = ( raw.x > max.x ) ? raw.x : max.x;
max.y = ( raw.y > max.y ) ? raw.y : max.y;
max.z = ( raw.z > max.z ) ? raw.z : max.z;
min.x = ( avg_calib_sample.x < min.x ) ? avg_calib_sample.x : min.x;
min.y = ( avg_calib_sample.y < min.y ) ? avg_calib_sample.y : min.y;
min.z = ( avg_calib_sample.z < min.z ) ? avg_calib_sample.z : min.z;
max.x = ( avg_calib_sample.x > max.x ) ? avg_calib_sample.x : max.x;
max.y = ( avg_calib_sample.y > max.y ) ? avg_calib_sample.y : max.y;
max.z = ( avg_calib_sample.z > max.z ) ? avg_calib_sample.z : max.z;

const int16_t minval = (32768/100)*1; // 1% full scale
if( abs(raw.x) < minval && abs(raw.y) < minval && variance < 200.f && raw.z > 2*minval )
if( abs(avg_calib_sample.x) < minval && abs(avg_calib_sample.y) < minval && variance < 200.f && avg_calib_sample.z > 2*minval )
bits.zmax_green = true;
if( abs(raw.x) < minval && abs(raw.y) < minval && variance < 200.f && raw.z < -2*minval )
if( abs(avg_calib_sample.x) < minval && abs(avg_calib_sample.y) < minval && variance < 200.f && avg_calib_sample.z < -2*minval )
bits.zmin_green = true;
if( abs(raw.x) < minval && abs(raw.z) < minval && variance < 200.f && raw.y > 2*minval )
if( abs(avg_calib_sample.x) < minval && abs(avg_calib_sample.z) < minval && variance < 200.f && avg_calib_sample.y > 2*minval )
bits.ymax_green = true;
if( abs(raw.x) < minval && abs(raw.z) < minval && variance < 200.f && raw.y < -2*minval )
if( abs(avg_calib_sample.x) < minval && abs(avg_calib_sample.z) < minval && variance < 200.f && avg_calib_sample.y < -2*minval )
bits.ymin_green = true;
if( abs(raw.y) < minval && abs(raw.z) < minval && variance < 200.f && raw.x > 2*minval )
if( abs(avg_calib_sample.y) < minval && abs(avg_calib_sample.z) < minval && variance < 200.f && avg_calib_sample.x > 2*minval )
bits.xmax_green = true;
if( abs(raw.y) < minval && abs(raw.z) < minval && variance < 200.f && raw.x < -2*minval )
if( abs(avg_calib_sample.y) < minval && abs(avg_calib_sample.z) < minval && variance < 200.f && avg_calib_sample.x < -2*minval )
bits.xmin_green = true;

if( i < 2 )
if( nrsamples < 2 )
return;

// Calculate hard iron correction
Expand Down Expand Up @@ -295,15 +296,15 @@ bool Compass::calibrate( bool (*reporter)( t_magn_axes raw, t_float_axes scale,
min = { 20000,20000,20000 } ;
max = { 0,0,0 };
bits = { false, false, false, false, false, false };
raw = { 0,0,0 };
avgX = new Average<20, int16_t>;
avgY = new Average<20, int16_t>;
avgZ = new Average<20, int16_t>;
i=0;
avg_calib_sample = { 0,0,0 };
avgX = new Average<10, int16_t>;
avgY = new Average<10, int16_t>;
avgZ = new Average<10, int16_t>;
nrsamples=0;
while( true )
{
// Send a calibration report to the subscriber
reporter( raw, scale, bias, bits, false );
reporter( avg_calib_sample, scale, bias, bits, false );
if( ESPRotary::readSwitch() == true ) // more responsive to query every loop
break;
delay(10);
Expand All @@ -316,13 +317,13 @@ bool Compass::calibrate( bool (*reporter)( t_magn_axes raw, t_float_axes scale,
}else{
ESP_LOGI( FNAME, "Show Calibration");
t_bitfield_compass b = calibration_bits.get();
reporter( axes, scale, bias, b, true );
reporter( magRaw, scale, bias, b, true );
while( ESPRotary::readSwitch() == true )
delay(100);
}
if( !only_show ){
ESP_LOGI( FNAME, "Read Cal-Samples=%d, OK=%d, NOK=%d", i, i-errors, errors );
if( i < 2 )
ESP_LOGI( FNAME, "Read Cal-Samples=%d, OK=%d, NOK=%d", nrsamples, nrsamples-errors, errors );
if( nrsamples < 2 )
{
// Too less samples to start calibration
ESP_LOGI( FNAME, "calibrate min-max xyz not enough samples");
Expand Down Expand Up @@ -433,69 +434,65 @@ float Compass::heading( bool *ok )
// ESP_LOGI(FNAME,"Errors overrun, return 0");
return 0.0;
}
if( errors > 100 ){
if( !holddown ){
holddown = true;
ESP_LOGI(FNAME,"Permanent compass sensor read errors: Start Holddown");
}
}else{
if( holddown ){
holddown = false;
ESP_LOGI(FNAME,"Okay again compass sensor read");

bool state = false;
if ( sensor->isCalibrated() ) {
vector_ijk tmp;
state = sensor->readBiased( tmp );
if ( state ) {
// rotate -90Z and then 180X, to have the same orientation as the IMU reference system
fx = -tmp.b;
fy = -tmp.a;
fz = -tmp.c;
}
}
bool state = sensor->rawAxes( rawAxes );
// ESP_LOGI(FNAME,"state %d x:%d y:%d z:%d", state, rawAxes.x, rawAxes.y, rawAxes.z );
if( !state )
{
errors++;
age++;
totalReadErrors++;
// ESP_LOGI(FNAME,"Magnetic sensor error Reads:%d, Total Errors:%d Init: %d", N, totalReadErrors, errors );
if( errors > 10 )
else {
state = sensor->readRaw( magRaw );
// ESP_LOGI(FNAME,"state %d x:%d y:%d z:%d", state, magRaw.x, magRaw.y, magRaw.z );
if( !state )
{
// ESP_LOGI(FNAME,"Magnetic sensor errors > 10: init mag sensor" );
if( compass_enable.get() != CS_CAN ){
if( sensor->initialize() != ESP_OK ) // reinitialize once crashed, one retry
sensor->initialize();
errors++;
age++;
totalReadErrors++;
// ESP_LOGI(FNAME,"Magnetic sensor error Reads:%d, Total Errors:%d Init: %d", N, totalReadErrors, errors );
if( errors > 10 )
{
// ESP_LOGI(FNAME,"Magnetic sensor errors > 10: init mag sensor" );
if( compass_enable.get() != CS_CAN ){
if( sensor->initialize() != ESP_OK ) // reinitialize once crashed, one retry
sensor->initialize();
}
return 0.0;
}
return 0.0;
}
if( errors > 100 ){ // survive 100 samples with constant heading if no valid reading
return 0.0;
if( errors > 100 ){ // survive 100 samples with constant heading if no valid reading
return 0.0;
}
*ok = true;
return _heading;
}
*ok = true;
return _heading;
// rotate -90Z and then 180X, to have the same orientation as the IMU reference system
fx = - ((float)magRaw.y - bias.y) * scale.y;
fy = - ((float)magRaw.x - bias.x) * scale.x;
fz = - ((float)magRaw.z - bias.z) * scale.z;
}
errors = 0;
age = 0;

/* Apply tilt corrections to the measured values. Note, due to mounting of chip
* turned clockwise by 90 degrees the X-axis and the Y-axis are moved and
* have to be handled in this way.
*/
// ESP_LOGI( FNAME, "heading: X:%d Y:%d Z:%d xs:%f ys:%f zs:%f", rawAxes.x, rawAxes.y, rawAxes.z, scale.x, scale.y, scale.z);

fx = -(double) ((float( rawAxes.y ) - bias.y) * scale.y); // mounting correction
fy = -(double) ((float( rawAxes.x ) - bias.x) * scale.x);
fz = -(double) ((float( rawAxes.z ) - bias.z) * scale.z);
// ESP_LOGI(FNAME, "raw x %d, y %d, z %d ", rawAxes.x, rawAxes.y, rawAxes.z);

vector_ijk mv( fx,fy,fz ); // magnetic vector, relative to glider from raw hall sensor x/y/z data
vector_ijk mev = IMU::getAHRSQuaternion().conjugate() * mv; // rotate magnetic vector
vector_ijk mv( fx,fy,fz ); // uT magnetic vector, relative to glider
vector_ijk mev = IMU::getAHRSQuaternion().conjugate() * mv; // rotate magnetic vector back in ENU ref sys
// ESP_LOGI(FNAME, "gravity a %.2f, b %.2f, c %.2f MV: a %.2f, b %.2f, c %.2f ", gravity_vector.a, gravity_vector.b, gravity_vector.c, mv.a, mv.b, mv.c );
// ESP_LOGI(FNAME, "gravity a %.2f, b %.2f, c %.2f ME a %.2f, b %.2f, c %.2f MEV: a %.2f, b %.2f, c %.2f ", gravity_vector.a, gravity_vector.b, gravity_vector.c, mv.a, mv.b, mv.c, mev.a, mev.b, mev.c );
// ESP_LOGI(FNAME, "rot a %.2f, b %.2f, c %.2f, w %.2f - %.2f ", q.b, q.c, q.d, q.a, RAD_TO_DEG*q.getAngle() );
_heading = Compass_atan2( mev.b, mev.a );
// ESP_LOGI(FNAME,"compensated mag a %.2f, b %.2f, c %.2f h %.2f", mev.a, mev.b, mev.c, _heading);
// ESP_LOGI(FNAME,"mag (%.2f,%.2f,%.2f) heading %.2f", mev.a, mev.b, mev.c, _heading);

_heading = Vector::normalizeDeg( _heading ); // normalize the +-180 degree model to 0..360°

// ESP_LOGI(FNAME,"Magn-Eul:(Y:%.1f P:%.1f R:%.1f) Magn-Vec:(%.4f %.4f %.4f) G-Vec:(%.4f/%.4f/%.4f) G-Eul:(P:%.1f R:%.1f)", _heading, ce.pitch, ce.roll , mv.a, mv.b, mv.c, gravity_vector.a,gravity_vector.b,gravity_vector.c, IMU::getPitch(), IMU::getRoll() );
#if 0
if( wind_logging.get() ){
char log[120];
sprintf( log, "$COMPASS;%d;%d;%d;%.1f;%.1f;%.1f;%d\n", rawAxes.x, rawAxes.y, rawAxes.z, IMU::getPitch(), IMU::getRoll(), _heading, totalReadErrors );
sprintf( log, "$COMPASS;%d;%d;%d;%.1f;%.1f;%.1f;%d\n", magRaw.x, magRaw.y, magRaw.z, IMU::getPitch(), IMU::getRoll(), _heading, totalReadErrors );
Router::sendXCV( log );
}
#endif
Expand Down
35 changes: 16 additions & 19 deletions main/Compass.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ class Compass: public Deviation
float calY() { return ((float( (float)sensor->curY() ) - bias.y) * scale.y); };
float calZ() { return ((float( (float)sensor->curZ() ) - bias.z) * scale.z); };

t_magn_axes getRawAxes() { return rawAxes; };
t_magn_axes getRawAxes() { return magRaw; };
float filteredHeading( bool *okIn );
float filteredTrueHeading( bool *okIn, bool withDeviation=true );
void setGyroHeading( float hd );
Expand Down Expand Up @@ -133,31 +133,28 @@ class Compass: public Deviation
int gyro_age;

/** Variables used by calibration. */
t_magn_axes avg_calib_sample;
t_float_axes bias;
t_float_axes scale;
t_magn_axes min;
t_magn_axes max;
Average<10, int16_t> *avgX = 0;
Average<10, int16_t> *avgY = 0;
Average<10, int16_t> *avgZ = 0;
bool calibrationRunning;
float _heading;
bool holddown;
int nrsamples;
t_bitfield_compass bits;

// Error counters
int errors;
int totalReadErrors;
Average<20, float, float> filterRoll;
Average<20, float, float> filterPitch;

// Mag readings
int age;
MagnetSensor *sensor;
t_magn_axes can;
double fx;
double fy;
double fz;
t_magn_axes rawAxes;
t_magn_axes raw;
t_magn_axes axes;
t_bitfield_compass bits;
t_magn_axes min;
t_magn_axes max;
Average<20, int16_t> *avgX = 0;
Average<20, int16_t> *avgY = 0;
Average<20, int16_t> *avgZ = 0;
int i;
float fx; //bias corrected
float fy;
float fz;
float _heading;
t_magn_axes magRaw;
};
10 changes: 5 additions & 5 deletions main/KalmanMPU6050.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,10 +195,10 @@ void IMU::Process()
float lf = loadFactor > 2.0 ? 2.0 : loadFactor;
loadFactor = lf < 0 ? 0 : lf; // limit to 0..2g
// the yz portion of w is proportional to the length of YZ portion of the normalized axis.
circle_omega = w * sqrt(axis.b*axis.b + axis.c*axis.c);
circle_omega = w * sqrt(axis.b*axis.b + axis.c*axis.c) * (std::signbit(gyro_rad.c)?-1.f:1.f);
// tan(roll):= petal force/G = m w v / m g
float tanw = circle_omega * getTAS() / (3.6f * 9.80665f);
roll = (std::signbit(gyro_rad.c)?1.f:-1.f) * atan( tanw );
float tanw = -circle_omega * getTAS() / (3.6f * 9.80665f);
roll = atan( tanw );
if ( ahrs_roll_check.get() ) {
// expected extra load c = sqrt(aa+bb) - 1, here a = 1/9.81 x atan, b=1
float loadz_exp = sqrt(tanw*tanw/(9.80665f*9.80665f)+1.f) - 1.f;
Expand Down Expand Up @@ -350,7 +350,7 @@ float IMU::getVerticalAcceleration()

float IMU::getVerticalOmega()
{
return (std::signbit(gyro.c)?1.f:-1.f) * circle_omega;
return circle_omega;
}

double IMU::PitchFromAccel()
Expand Down Expand Up @@ -389,7 +389,7 @@ class IMU_Ref
double Nl = (Bl - tmp).get_norm();

// ESP_LOGI(FNAME, "iter: %f,%f,%f", x[0],x[1],x[2]);
return pow(Nr-1,2) + pow(Nl-1,2) + pow(Nl- Nr,2)/10.;
return pow(Nr-1,2) + pow(Nl-1,2) + (tmp).get_norm2() + pow(Nl- Nr,2)/10.;
}
private:
vector_d Br, Bl;
Expand Down
Loading

0 comments on commit bfc3f37

Please sign in to comment.