#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <stdlib.h>
#include "analog.h"
#include "configuration.h"
#include "attitude.h"
#include "printf_P.h"
#include "isqrt.h"
#include "twimaster.h"
// for Delay functions
#include "timer0.h"
// For reading and writing acc. meter offsets.
#include "eeprom.h"
// For debugOut
#include "output.h"
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
#define startADC() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
const char* recal
= ", recalibration needed.";
volatile uint16_t ADSensorInputs
[8];
/*
* These 4 exported variables are zero-offset. The "PID" ones are used
* in the attitude control as rotation rates. The "ATT" ones are for
* integration to angles.
*/
int16_t gyro_PID
[3];
int16_t gyro_ATT
[3];
int16_t gyroD
[3];
int16_t gyroDWindow
[3][GYRO_D_WINDOW_LENGTH
];
uint8_t gyroDWindowIdx
= 0;
/*
* Airspeed
*/
int16_t airpressure
;
uint16_t airspeedVelocity
= 0;
//int16_t airpressureWindow[AIRPRESSURE_WINDOW_LENGTH];
//uint8_t airpressureWindowIdx = 0;
/*
* Offset values. These are the raw gyro and acc. meter sums when the copter is
* standing still. They are used for adjusting the gyro and acc. meter values
* to be centered on zero.
*/
sensorOffset_t gyroOffset
;
uint16_t airpressureOffset
;
/*
* In the MK coordinate system, nose-down is positive and left-roll is positive.
* If a sensor is used in an orientation where one but not both of the axes has
* an opposite sign, PR_ORIENTATION_REVERSED is set to 1 (true).
* Transform:
* pitch <- pp*pitch + pr*roll
* roll <- rp*pitch + rr*roll
* Not reversed, GYRO_QUADRANT:
* 0: pp=1, pr=0, rp=0, rr=1 // 0 degrees
* 1: pp=1, pr=-1,rp=1, rr=1 // +45 degrees
* 2: pp=0, pr=-1,rp=1, rr=0 // +90 degrees
* 3: pp=-1,pr=-1,rp=1, rr=1 // +135 degrees
* 4: pp=-1,pr=0, rp=0, rr=-1 // +180 degrees
* 5: pp=-1,pr=1, rp=-1,rr=-1 // +225 degrees
* 6: pp=0, pr=1, rp=-1,rr=0 // +270 degrees
* 7: pp=1, pr=1, rp=-1,rr=1 // +315 degrees
* Reversed, GYRO_QUADRANT:
* 0: pp=-1,pr=0, rp=0, rr=1 // 0 degrees with pitch reversed
* 1: pp=-1,pr=-1,rp=-1,rr=1 // +45 degrees with pitch reversed
* 2: pp=0, pr=-1,rp=-1,rr=0 // +90 degrees with pitch reversed
* 3: pp=1, pr=-1,rp=-1,rr=1 // +135 degrees with pitch reversed
* 4: pp=1, pr=0, rp=0, rr=-1 // +180 degrees with pitch reversed
* 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed
* 6: pp=0, pr=1, rp=1, rr=0 // +270 degrees with pitch reversed
* 7: pp=-1,pr=1, rp=1, rr=1 // +315 degrees with pitch reversed
*/
void rotate
(int16_t* result
, uint8_t quadrant
, uint8_t reversePR
, uint8_t reverseYaw
) {
static const int8_t rotationTab
[] = {1,1,0,-1,-1,-1,0,1};
// Pitch to Pitch part
int8_t xx
= reversePR
? rotationTab
[(quadrant
+4)%8] : rotationTab
[quadrant
];
// Roll to Pitch part
int8_t xy
= rotationTab
[(quadrant
+2)%8];
// Pitch to Roll part
int8_t yx
= reversePR
? rotationTab
[(quadrant
+2)%8] : rotationTab
[(quadrant
+6)%8];
// Roll to Roll part
int8_t yy
= rotationTab
[quadrant
];
int16_t xIn
= result
[0];
result
[0] = xx
*xIn
+ xy
*result
[1];
result
[1] = yx
*xIn
+ yy
*result
[1];
if (quadrant
& 1) {
// A rotation was used above, where the factors were too large by sqrt(2).
// So, we multiply by 2^n/sqt(2) and right shift n bits, as to divide by sqrt(2).
// A suitable value for n: Sample is 11 bits. After transformation it is the sum
// of 2 11 bit numbers, so 12 bits. We have 4 bits left...
result
[0] = (result
[0]*11) >> 4;
result
[1] = (result
[1]*11) >> 4;
}
if (reverseYaw
)
result
[3] =-result
[3];
}
/*
* Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt.
* That is divided by 3 below, for a final 10.34 per volt.
* So the initial value of 100 is for 9.7 volts.
*/
uint16_t UBat
= 100;
/*
* Control and status.
*/
volatile uint8_t sensorDataReady
= ALL_DATA_READY
;
/*
* Experiment: Measuring vibration-induced sensor noise.
*/
uint16_t gyroNoisePeak
[3];
volatile uint8_t adState
;
volatile uint8_t adChannel
;
// ADC channels
#define AD_UBAT 6
#define AD_AIRPRESSURE 7
/*
* Table of AD converter inputs for each state.
* The number of samples summed for each channel is equal to
* the number of times the channel appears in the array.
* The max. number of samples that can be taken in 2 ms is:
* 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control
* loop needs a little time between reading AD values and
* re-enabling ADC, the real limit is (how much?) lower.
* The acc. sensor is sampled even if not used - or installed
* at all. The cost is not significant.
*/
const uint8_t channelsForStates
[] PROGMEM
= {
AD_AIRPRESSURE
,
AD_UBAT
,
AD_AIRPRESSURE
,
AD_AIRPRESSURE
,
AD_AIRPRESSURE
,
};
// Feature removed. Could be reintroduced later - but should work for all gyro types then.
// uint8_t GyroDefectPitch = 0, GyroDefectRoll = 0, GyroDefectYaw = 0;
void analog_init
(void) {
uint8_t sreg
= SREG
;
// disable all interrupts before reconfiguration
cli
();
// ADC0 ... ADC7 is connected to PortA pin 0 ... 7
// DDRA = 0x00;
// PORTA = 0x00;
// Digital Input Disable Register 0
// Disable digital input buffer for analog adc_channel pins
// DIDR0 = 0xFF;
// external reference, adjust data to the right
ADMUX
&= ~
((1<<REFS1
)|(1<<REFS0
)|(1<<ADLAR
));
// set muxer to ADC adc_channel 0 (0 to 7 is a valid choice)
ADMUX
= (ADMUX
& 0xE0);
//Set ADC Control and Status Register A
//Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz
ADCSRA
= (1<<ADPS2
)|(1<<ADPS1
)|(1<<ADPS0
);
//Set ADC Control and Status Register B
//Trigger Source to Free Running Mode
ADCSRB
&= ~
((1<<ADTS2
)|(1<<ADTS1
)|(1<<ADTS0
));
startAnalogConversionCycle
();
// restore global interrupt flags
SREG
= sreg
;
}
/*
* Here the axes of the sensor can be shuffled around.
*/
uint16_t rawGyroValue
(uint8_t axis
) {
return IMU3200SensorInputs
[axis
+1]; // skip temperature mesaurement in any case..
}
/*
uint16_t rawAccValue(uint8_t axis) {
return sensorInputs[AD_ACC_PITCH-axis];
}
*/
void measureNoise
(const int16_t sensor
,
volatile uint16_t* const noiseMeasurement
, const uint8_t damping
) {
if (sensor
> (int16_t) (*noiseMeasurement
)) {
*noiseMeasurement
= sensor
;
} else if (-sensor
> (int16_t) (*noiseMeasurement
)) {
*noiseMeasurement
= -sensor
;
} else if (*noiseMeasurement
> damping
) {
*noiseMeasurement
-= damping
;
} else {
*noiseMeasurement
= 0;
}
}
void startAnalogConversionCycle
(void) {
twimaster_startCycle
();
// Stop the sampling. Cycle is over.
for (uint8_t i
= 0; i
<8; i
++) {
ADSensorInputs
[i
] = 0;
}
adState
= 0;
adChannel
= AD_AIRPRESSURE
;
ADMUX
= (ADMUX
& 0xE0) | adChannel
;
startADC
();
sensorDataReady
= 0;
}
/*****************************************************
* Interrupt Service Routine for ADC
* Runs at 312.5 kHz or 3.2 �s. When all states are
* processed further conversions are stopped.
*****************************************************/
ISR
(ADC_vect
) {
ADSensorInputs
[adChannel
] += ADC
;
// set up for next state.
adState
++;
if (adState
< sizeof(channelsForStates
)) {
adChannel
= pgm_read_byte
(&channelsForStates
[adState
]);
// set adc muxer to next adChannel
ADMUX
= (ADMUX
& 0xE0) | adChannel
;
// after full cycle stop further interrupts
startADC
();
} else {
sensorDataReady
|= ADC_DATA_READY
;
// do not restart ADC converter.
}
}
void analog_updateGyros
(void) {
// for various filters...
int16_t tempOffsetGyro
[3], tempGyro
;
for (uint8_t axis
=0; axis
<3; axis
++) {
tempGyro
= rawGyroValue
(axis
);
/*
* Process the gyro data for the PID controller.
*/
// Saturation prevention was removed. No airplane rotates more than 2000 deg/s.
// 2) Apply sign and offset, scale before filtering.
tempOffsetGyro
[axis
] = (tempGyro
- gyroOffset.
offsets[axis
]);
}
// 2.1: Transform axes.
rotate
(tempOffsetGyro
, IMUConfig.
gyroQuadrant, IMUConfig.
imuReversedFlags & IMU_REVERSE_GYRO_PR
, IMUConfig.
imuReversedFlags & IMU_REVERSE_GYRO_YAW
);
for (uint8_t axis
=0; axis
<3; axis
++) {
// 3) Filter.
tempOffsetGyro
[axis
] = (gyro_PID
[axis
] * (IMUConfig.
gyroPIDFilterConstant - 1) + tempOffsetGyro
[axis
]) / IMUConfig.
gyroPIDFilterConstant;
// 4) Measure noise.
measureNoise
(tempOffsetGyro
[axis
], &gyroNoisePeak
[axis
], GYRO_NOISE_MEASUREMENT_DAMPING
);
// 5) Differential measurement.
// TODO: Examine effects of overruns here, they are quite possible.
int16_t diff
= tempOffsetGyro
[axis
] - gyro_PID
[axis
];
gyroD
[axis
] -= gyroDWindow
[axis
][gyroDWindowIdx
];
gyroD
[axis
] += diff
;
gyroDWindow
[axis
][gyroDWindowIdx
] = diff
;
// 6) Done.
gyro_PID
[axis
] = tempOffsetGyro
[axis
];
// Prepare tempOffsetGyro for next calculation below...
tempOffsetGyro
[axis
] = (rawGyroValue
(axis
) - gyroOffset.
offsets[axis
]);
}
/*
* Now process the data for attitude angles.
*/
rotate
(tempOffsetGyro
, IMUConfig.
gyroQuadrant, IMUConfig.
imuReversedFlags & IMU_REVERSE_GYRO_PR
, IMUConfig.
imuReversedFlags & IMU_REVERSE_GYRO_YAW
);
// dampenGyroActivity();
gyro_ATT
[PITCH
] = tempOffsetGyro
[PITCH
];
gyro_ATT
[ROLL
] = tempOffsetGyro
[ROLL
];
gyro_ATT
[YAW
] = tempOffsetGyro
[YAW
];
if (++gyroDWindowIdx
>= IMUConfig.
gyroDWindowLength) {
gyroDWindowIdx
= 0;
}
}
// probably wanna aim at 1/10 m/s/unit.
#define LOG_AIRSPEED_FACTOR 0
void analog_updateAirspeed
(void) {
uint16_t rawAirpressure
= ADSensorInputs
[AD_AIRPRESSURE
];
int16_t temp
= rawAirpressure
- airpressureOffset
;
// airpressure -= airpressureWindow[airpressureWindowIdx];
// airpressure += temp;
// airpressureWindow[airpressureWindowIdx] = temp;
// airpressureWindowIdx++;
// if (airpressureWindowIdx == AIRPRESSURE_WINDOW_LENGTH) {
// airpressureWindowIdx = 0;
// }
#define AIRPRESSURE_FILTER 16
airpressure
= ((int32_t)airpressure
* (AIRPRESSURE_FILTER
-1) + (AIRPRESSURE_FILTER
/2) + temp
) / AIRPRESSURE_FILTER
;
uint16_t p2
= (airpressure
<0) ? 0 : airpressure
;
airspeedVelocity
= (staticParams.
airspeedCorrection * isqrt16
(p2
)) >> LOG_AIRSPEED_FACTOR
;
debugOut.
analog[17] = airpressure
;
debugOut.
analog[18] = airpressureOffset
;
debugOut.
analog[19] = airspeedVelocity
;
isFlying
= 0; //(airspeedVelocity >= staticParams.isFlyingThreshold);
}
void analog_updateBatteryVoltage
(void) {
// Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
// This is divided by 3 --> 10.34 counts per volt.
UBat
= (3 * UBat
+ ADSensorInputs
[AD_UBAT
] / 3) / 4;
}
void analog_update
(void) {
analog_updateGyros
();
// analog_updateAccelerometers();
analog_updateAirspeed
();
analog_updateBatteryVoltage
();
#ifdef USE_MK3MAG
magneticHeading
= volatileMagneticHeading
;
#endif
}
void analog_setNeutral
() {
twimaster_setNeutral
();
if (gyroOffset_readFromEEProm
()) {
printf("gyro offsets invalid%s",recal
);
gyroOffset.
offsets[PITCH
] = gyroOffset.
offsets[ROLL
] = 512 * GYRO_OVERSAMPLING
;
gyroOffset.
offsets[YAW
] = 512 * GYRO_OVERSAMPLING
;
}
// Noise is relative to offset. So, reset noise measurements when changing offsets.
for (uint8_t i
=PITCH
; i
<=YAW
; i
++) {
gyroNoisePeak
[i
] = 0;
gyroD
[i
] = 0;
for (uint8_t j
=0; j
<GYRO_D_WINDOW_LENGTH
; j
++) {
gyroDWindow
[i
][j
] = 0;
}
}
// Setting offset values has an influence in the analog.c ISR
// Therefore run measurement for 100ms to achive stable readings
delay_ms_with_adc_measurement
(100, 0);
// gyroActivity = 0;
}
void analog_calibrate
(void) {
#define OFFSET_CYCLES 120
uint8_t i
, axis
;
int32_t offsets
[4] = { 0, 0, 0, 0};
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
for (i
= 0; i
< OFFSET_CYCLES
; i
++) {
delay_ms_with_adc_measurement
(10, 1);
for (axis
= PITCH
; axis
<= YAW
; axis
++) {
offsets
[axis
] += rawGyroValue
(axis
);
}
offsets
[3] += ADSensorInputs
[AD_AIRPRESSURE
];
}
for (axis
= PITCH
; axis
<= YAW
; axis
++) {
gyroOffset.
offsets[axis
] = (offsets
[axis
] + OFFSET_CYCLES
/ 2) / OFFSET_CYCLES
;
}
airpressureOffset
= (offsets
[3] + OFFSET_CYCLES
/ 2) / OFFSET_CYCLES
;
int16_t min
= 200;
int16_t max
= 1024-200;
if(airpressureOffset
< min
|| airpressureOffset
> max
)
versionInfo.
hardwareErrors[0] |= FC_ERROR0_PRESSURE
;
gyroOffset_writeToEEProm
();
startAnalogConversionCycle
();
}