Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

An implenentaion of nav-rewrite (squashed commit, November 17th).

Browse source 
Automatic magnetic declination from GPS coordinates (approximate to 1-2 degrees) (disabled)
Accelerometer bias compensation
Quaternion-based Mahony's DCM IMU (implementation by S. Madgwick)
Ability to lock heading to GPS cource for airplanes
Faster acquisition by initial attitude by using higher DCM kP gain on powerup
Changes to mag calibration algorithm (@HaukeRa's code). Compass calibration now independent from align_mag.
Changed acc calibration to proper 3-axis offset calculation code (same code as for mag). Changed board alignment angles to decidegrees (configurator incompatibility). Removed accelerometer trims (values passed thru MSP are ignored). Accel trim stick combo now adjusts board alignment directly (real-time, without reboot). Removed inflight acc calibration.
FIR filter implementation for different looptimes. Option gyro_cut_hz replaced with gyro_fir_enable (default gyro_soft_filter=3 preset - a very pessimistic filter with fairly large delay but low cutoff frequency for large or very noisy copters)
iNav-related PIDs renamed in CLI to avoid re-usage of old values when restoring config dumps.
PID with back-calculation and I-term anti-windup. Smarter max acceleration and jerk limiting
Initial support for airplane navigation. ALTHOLD, POSHOLD (circular loiter around the waypoint) and RTH should work. UNTESTED! RTH altitude probably needs some work. Autolanding will NOT function at the moment - the AUTOLAND phase will never complete. Absolutely no throttle control at the moment (fully manual control).
Increase delay for bmp085 baro detection. Resolves an issue with bmp085 not being detected sometimes on powerup
Fixed the missing checks for MIXER_CUSTOM_AIRPLANE
Lots of small fixes
This commit is contained in:
digitalentity 2015-04-27 00:29:29 +10:00 • committed by Konstantin Sharlaimov (DigitalEntity)
parent 1a8657c7b7
commit 34046169c7
73 changed files with 6262 additions and 2340 deletions
Download patch file Download diff file Expand all files Collapse all files

205
src/main/sensors/acceleration.c
View file

@ -17,10 +17,13 @@
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "platform.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
@ -42,13 +45,10 @@ uint16_t acc_1G = 256; // this is the 1G measured acceleration.
uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
extern uint16_t InflightcalibratingA;
extern bool AccInflightCalibrationArmed;
extern bool AccInflightCalibrationMeasurementDone;
extern bool AccInflightCalibrationSavetoEEProm;
extern bool AccInflightCalibrationActive;
static flightDynamicsTrims_t *accelerationTrims;
static flightDynamicsTrims_t * accZero;
static flightDynamicsTrims_t * accGain;
static int8_t * accFIRTable = 0L;
static int16_t accFIRState[3][9];
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
@ -70,38 +70,93 @@ bool isOnFirstAccelerationCalibrationCycle(void)
return calibratingA == CALIBRATING_ACC_CYCLES;
}
void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
static sensorCalibrationState_t calState;
static bool calibratedAxis[6];
static int32_t accSamples[6][3];
static int calibratedAxisCount = 0;
int getPrimaryAxisIndex(int16_t sample[3])
{
rollAndPitchTrims->values.roll = 0;
rollAndPitchTrims->values.pitch = 0;
if (ABS(sample[Z]) > ABS(sample[X]) && ABS(sample[Z]) > ABS(sample[Y])) {
//Z-axis
return (sample[Z] > 0) ? 0 : 1;
}
else if (ABS(sample[X]) > ABS(sample[Y]) && ABS(sample[X]) > ABS(sample[Z])) {
//X-axis
return (sample[X] > 0) ? 2 : 3;
}
else if (ABS(sample[Y]) > ABS(sample[X]) && ABS(sample[Y]) > ABS(sample[Z])) {
//Y-axis
return (sample[Y] > 0) ? 4 : 5;
}
else
return -1;
}
void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
void performAcclerationCalibration(void)
{
static int32_t a[3];
int axisIndex = getPrimaryAxisIndex(accADC);
uint8_t axis;
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (isOnFirstAccelerationCalibrationCycle())
a[axis] = 0;
// Sum up CALIBRATING_ACC_CYCLES readings
a[axis] += accADC[axis];
// Reset global variables to prevent other code from using un-calibrated data
accADC[axis] = 0;
accelerationTrims->raw[axis] = 0;
// Check if sample is usable
if (axisIndex < 0) {
return;
}
if (isOnFinalAccelerationCalibrationCycle()) {
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
accelerationTrims->raw[X] = (a[X] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
accelerationTrims->raw[Y] = (a[Y] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
accelerationTrims->raw[Z] = (a[Z] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
// Top-up and first calibration cycle, reset everything
if (axisIndex == 0 && isOnFirstAccelerationCalibrationCycle()) {
for (axis = 0; axis < 6; axis++) {
calibratedAxis[axis] = false;
accSamples[axis][X] = 0;
accSamples[axis][Y] = 0;
accSamples[axis][Z] = 0;
}
resetRollAndPitchTrims(rollAndPitchTrims);
calibratedAxisCount = 0;
sensorCalibrationResetState(&calState);
}
if (!calibratedAxis[axisIndex]) {
sensorCalibrationPushSampleForOffsetCalculation(&calState, accADC);
accSamples[axisIndex][X] += accADC[X];
accSamples[axisIndex][Y] += accADC[Y];
accSamples[axisIndex][Z] += accADC[Z];
if (isOnFinalAccelerationCalibrationCycle()) {
calibratedAxis[axisIndex] = true;
calibratedAxisCount++;
beeperConfirmationBeeps(2);
}
}
if (calibratedAxisCount == 6) {
float accTmp[3];
int16_t accSample16[3];
/* Calculate offset */
sensorCalibrationSolveForOffset(&calState, accTmp);
for (axis = 0; axis < 3; axis++) {
accZero->raw[axis] = lrintf(accTmp[axis]);
}
/* Not we can offset our accumulated averages samples and calculate scale factors and calculate gains */
sensorCalibrationResetState(&calState);
for (axis = 0; axis < 6; axis++) {
accSample16[X] = accSamples[axis][X] / CALIBRATING_ACC_CYCLES - accZero->raw[X];
accSample16[Y] = accSamples[axis][Y] / CALIBRATING_ACC_CYCLES - accZero->raw[Y];
accSample16[Z] = accSamples[axis][Z] / CALIBRATING_ACC_CYCLES - accZero->raw[Z];
sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample16, acc_1G);
}
sensorCalibrationSolveForScale(&calState, accTmp);
for (axis = 0; axis < 3; axis++) {
accGain->raw[axis] = lrintf(accTmp[axis] * 4096);
}
saveConfigAndNotify();
}
@ -109,85 +164,43 @@ void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
calibratingA--;
}
void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
void applyAccelerationZero(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain)
{
uint8_t axis;
static int32_t b[3];
static int16_t accZero_saved[3] = { 0, 0, 0 };
static rollAndPitchTrims_t angleTrim_saved = { { 0, 0 } };
// Saving old zeropoints before measurement
if (InflightcalibratingA == 50) {
accZero_saved[X] = accelerationTrims->raw[X];
accZero_saved[Y] = accelerationTrims->raw[Y];
accZero_saved[Z] = accelerationTrims->raw[Z];
angleTrim_saved.values.roll = rollAndPitchTrims->values.roll;
angleTrim_saved.values.pitch = rollAndPitchTrims->values.pitch;
}
if (InflightcalibratingA > 0) {
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (InflightcalibratingA == 50)
b[axis] = 0;
// Sum up 50 readings
b[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
accelerationTrims->raw[axis] = 0;
}
// all values are measured
if (InflightcalibratingA == 1) {
AccInflightCalibrationActive = false;
AccInflightCalibrationMeasurementDone = true;
beeper(BEEPER_ACC_CALIBRATION); // indicate end of calibration
// recover saved values to maintain current flight behaviour until new values are transferred
accelerationTrims->raw[X] = accZero_saved[X];
accelerationTrims->raw[Y] = accZero_saved[Y];
accelerationTrims->raw[Z] = accZero_saved[Z];
rollAndPitchTrims->values.roll = angleTrim_saved.values.roll;
rollAndPitchTrims->values.pitch = angleTrim_saved.values.pitch;
}
InflightcalibratingA--;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (AccInflightCalibrationSavetoEEProm) { // the aircraft is landed, disarmed and the combo has been done again
AccInflightCalibrationSavetoEEProm = false;
accelerationTrims->raw[X] = b[X] / 50;
accelerationTrims->raw[Y] = b[Y] / 50;
accelerationTrims->raw[Z] = b[Z] / 50 - acc_1G; // for nunchuck 200=1G
resetRollAndPitchTrims(rollAndPitchTrims);
saveConfigAndNotify();
}
accADC[X] = (accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096;
accADC[Y] = (accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096;
accADC[Z] = (accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096;
}
void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
{
accADC[X] -= accelerationTrims->raw[X];
accADC[Y] -= accelerationTrims->raw[Y];
accADC[Z] -= accelerationTrims->raw[Z];
}
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
void updateAccelerationReadings(void)
{
if (!acc.read(accADC)) {
return;
}
alignSensors(accADC, accADC, accAlign);
if (accFIRTable) {
filterApply9TapFIR(accADC, accFIRState, accFIRTable);
}
if (!isAccelerationCalibrationComplete()) {
performAcclerationCalibration(rollAndPitchTrims);
performAcclerationCalibration();
}
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
performInflightAccelerationCalibration(rollAndPitchTrims);
}
alignSensors(accADC, accADC, accAlign);
applyAccelerationTrims(accelerationTrims);
applyAccelerationZero(accZero, accGain);
}
void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
void setAccelerationZero(flightDynamicsTrims_t * accZeroToUse)
{
accelerationTrims = accelerationTrimsToUse;
accZero = accZeroToUse;
}
void setAccelerationGain(flightDynamicsTrims_t * accGainToUse)
{
accGain = accGainToUse;
}
void setAccelerationFilter(int8_t * filterTableToUse)
{
accFIRTable = filterTableToUse;
}