/branches/dongfang_FC_fixedwing/arduino_atmega328/ADXRS610_FC2.0.c |
---|
0,0 → 1,19 |
#include "ADXRS610_FC2.0.h" |
#include "configuration.h" |
void gyro_calibrate(void) { |
// Nothing to calibrate. |
} |
void gyro_init(void) { |
// No amplifiers, no DAC. |
} |
void gyro_setDefaultParameters(void) { |
IMUConfig.accQuadrant = 4; |
IMUConfig.imuReversedFlags = IMU_REVERSE_ACC_XY; |
staticParams.gyroD = 5; |
IMUConfig.driftCompDivider = 1; |
IMUConfig.driftCompLimit = 3; |
IMUConfig.zerothOrderCorrection = 1; |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/README.txt |
---|
0,0 → 1,27 |
Sub-version for an Arduino Pro Mini with an InvenSense ITG3200 3 axis I2C gyro. |
A 4017 decimal counter is used for servo control. |
I/O pin assignments: |
PB0/ICP1 PPM receiver signal in |
PB1,2 Digital outputs (output.c) |
PB3,4,5 Left free for SPI use |
PB6,7 Xtal |
PC0,1,2,3 unused |
PC5/SCL I2C to gyro |
PC4/SDA I2C to gyro |
PC6 RESET |
PC7 No pin?? |
PD0 RxD serial for configuration, diags etc. |
PD1 TxD serial for configuration, diags etc. |
PD2 unused |
PD3/OC2B: CLK of 4017 |
PD4: RESET of 4017 |
PD5 Beeper |
PD6,7 LEDs |
ADC6 Battery voltage divider |
ADC7 Airspeed sensor |
/branches/dongfang_FC_fixedwing/arduino_atmega328/analog.c |
---|
0,0 → 1,400 |
#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 "sensors.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 |
*/ |
uint32_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) { |
sensorDataReady = 0; |
// 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(); |
twimaster_startCycle(); |
} |
/***************************************************** |
* 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 2 |
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; |
} |
if (temp<0) temp = 0; |
airspeedVelocity = (staticParams.airspeedCorrection * isqrt32(airpressure)) >> LOG_AIRSPEED_FACTOR; |
isFlying = (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; |
} |
} |
for (uint8_t i=0; i<AIRPRESSURE_WINDOW_LENGTH; i++) { |
airpressureWindow[i] = 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 64 |
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; |
int16_t min = (512-200) * GYRO_OVERSAMPLING; |
int16_t max = (512+200) * GYRO_OVERSAMPLING; |
if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
} |
airpressureOffset = (offsets[3] + OFFSET_CYCLES / 2) / OFFSET_CYCLES; |
int16_t min = 200; |
int16_t max = (1024-200) * 2; |
if(airpressureOffset < min || airpressureOffset > max) |
versionInfo.hardwareErrors[0] |= FC_ERROR0_PRESSURE; |
gyroOffset_writeToEEProm(); |
startAnalogConversionCycle(); |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/analog.h |
---|
0,0 → 1,207 |
#ifndef _ANALOG_H |
#define _ANALOG_H |
#include <inttypes.h> |
#include "configuration.h" |
/* |
About setting constants for different gyros: |
Main parameters are positive directions and voltage/angular speed gain. |
The "Positive direction" is the rotation direction around an axis where |
the corresponding gyro outputs a voltage > the no-rotation voltage. |
A gyro is considered, in this code, to be "forward" if its positive |
direction is: |
- Nose down for pitch |
- Left hand side down for roll |
- Clockwise seen from above for yaw. |
Setting gyro gain correctly: All sensor measurements in analog.c take |
place in a cycle, each cycle comprising all sensors. Some sensors are |
sampled more than once (oversampled), and the results added. |
In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0) |
or 4 (other versions), offset to zero, low pass filtered and then assigned |
to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or |
roll. The factor 2 or 4 or whatever is called GYRO_FACTOR_PITCHROLL here. |
*/ |
/* |
GYRO_HW_FACTOR is the relation between rotation rate and ADCValue: |
ADCValue [units] = |
rotational speed [deg/s] * |
gyro sensitivity [V / deg/s] * |
amplifier gain [units] * |
1024 [units] / |
3V full range [V] |
GYRO_HW_FACTOR is: |
gyro sensitivity [V / deg/s] * |
amplifier gain [units] * |
1024 [units] / |
3V full range [V] |
Examples: |
FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7: |
GYRO_HW_FACTOR = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s). |
FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers: |
GYRO_HW_FACTOR = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s). |
My InvenSense copter has 2mV/deg/s gyros and no amplifiers: |
GYRO_HW_FACTOR = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s) |
(only about half as sensitive as V1.3. But it will take about twice the |
rotation rate!) |
GYRO_HW_FACTOR is given in the makefile. |
*/ |
/* |
* How many samples are added in one ADC loop, for pitch&roll and yaw, |
* respectively. This is = the number of occurences of each channel in the |
* channelsForStates array in analog.c. |
*/ |
#define GYRO_OVERSAMPLING 4 |
/* |
* The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate. |
*/ |
#define GYRO_RATE_FACTOR (GYRO_HW_FACTOR * GYRO_OVERSAMPLING) |
/* |
* The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor. |
* Will be about 10 or so for InvenSense, and about 33 for ADXRS610. |
*/ |
/* |
* Gyro saturation prevention. |
*/ |
// How far from the end of its range a gyro is considered near-saturated. |
#define SENSOR_MIN 32 |
// Other end of the range (calculated) |
#define SENSOR_MAX (GYRO_OVERSAMPLING * 1023 - SENSOR_MIN) |
// Max. boost to add "virtually" to gyro signal at total saturation. |
#define EXTRAPOLATION_LIMIT 2500 |
// Slope of the boost (calculated) |
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN) |
/* |
* This value is subtracted from the gyro noise measurement in each iteration, |
* making it return towards zero. |
*/ |
#define GYRO_NOISE_MEASUREMENT_DAMPING 5 |
#define PITCH 0 |
#define ROLL 1 |
#define YAW 2 |
//#define Z 2 |
/* |
* The values that this module outputs |
* These first 2 exported arrays are zero-offset. The "PID" ones are used |
* in the attitude control as rotation rates. The "ATT" ones are for |
* integration to angles. For the same axis, the PID and ATT variables |
* generally have about the same values. There are just some differences |
* in filtering, and when a gyro becomes near saturated. |
* Maybe this distinction is not really necessary. |
*/ |
extern int16_t gyro_PID[3]; |
extern int16_t gyro_ATT[3]; |
extern int16_t gyroD[3]; |
#define GYRO_D_WINDOW_LENGTH 8 |
extern uint16_t UBat; |
extern uint16_t airspeedVelocity; |
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3. |
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3)) |
extern sensorOffset_t gyroOffset; |
extern uint16_t airpressureOffset; |
/* |
* This is not really for external use - but the ENC-03 gyro modules needs it. |
*/ |
//extern volatile int16_t rawGyroSum[3]; |
/* |
* The acceleration values that this module outputs. They are zero based. |
*/ |
//extern int16_t acc[3]; |
//extern int16_t filteredAcc[3]; |
// extern volatile int32_t stronglyFilteredAcc[3]; |
/* |
* Diagnostics: Gyro noise level because of motor vibrations. The variables |
* only really reflect the noise level when the copter stands still but with |
* its motors running. |
*/ |
extern uint16_t gyroNoisePeak[3]; |
/* |
* Air pressure. |
* The sensor has a sensitivity of 45 mV/kPa. |
* An approximate p(h) formula is = p(h[m])[kPa] = p_0 - 11.95 * 10^-3 * h |
* p(h[m])[kPa] = 101.3 - 11.95 * 10^-3 * h |
* 11.95 * 10^-3 * h = 101.3 - p[kPa] |
* h = (101.3 - p[kPa])/0.01195 |
* That is: dV = -45 mV * 11.95 * 10^-3 dh = -0.53775 mV / m. |
* That is, with 38.02 * 1.024 / 3 steps per mV: -7 steps / m |
Display pressures |
4165 mV-->1084.7 |
4090 mV-->1602.4 517.7 |
3877 mV-->3107.8 1503.4 |
4165 mV-->1419.1 |
3503 mV-->208.1 |
Diff.: 1211.0 |
Calculated Vout = 5V(.009P-0.095) --> 5V .009P = Vout + 5V 0.095 --> P = (Vout + 5V 0.095)/(5V 0.009) |
4165 mV = 5V(0.009P-0.095) P = 103.11 kPa h = -151.4m |
4090 mV = 5V(0.009P-0.095) P = 101.44 kPa h = -11.7m 139.7m |
3877 mV = 5V(0.009P-0.095) P = 96.7 kPa h = 385m 396.7m |
4165 mV = 5V(0.009P-0.095) P = 103.11 kPa h = -151.4m |
3503 mV = 5V(0.009P-0.095) P = 88.4 kPa h = 384m Diff: 1079.5m |
Pressure at sea level: 101.3 kPa. voltage: 5V * (0.009P-0.095) = 4.0835V |
This is OCR2 = 143.15 at 1.5V in --> simple pressure = |
*/ |
#define AIRPRESSURE_WINDOW_LENGTH 32 |
extern uint16_t airspeedVelocity; |
/* |
* Flag: Interrupt handler has done all A/D conversion and processing. |
*/ |
#define ADC_DATA_READY 1 |
#define TWI_DATA_READY 2 |
#define ALL_DATA_READY (ADC_DATA_READY+TWI_DATA_READY) |
extern volatile uint8_t sensorDataReady; |
void analog_init(void); |
/* |
* This is really only for use for the ENC-03 code module, which needs to get the raw value |
* for its calibration. The raw value should not be used for anything else. |
*/ |
uint16_t rawGyroValue(uint8_t axis); |
/* |
* Start the conversion cycle. It will stop automatically. |
*/ |
void startAnalogConversionCycle(void); |
/* |
* Process the sensor data to update the exported variables. Must be called after each measurement cycle and before the data is used. |
*/ |
void analog_update(void); |
/* |
* Read gyro and acc.meter calibration from EEPROM. |
*/ |
void analog_setNeutral(void); |
/* |
* Zero-offset gyros and write the calibration data to EEPROM. |
*/ |
void analog_calibrate(void); |
#endif //_ANALOG_H |
/branches/dongfang_FC_fixedwing/arduino_atmega328/attitude.c |
---|
0,0 → 1,122 |
#include <stdlib.h> |
#include <avr/io.h> |
#include <stdlib.h> |
#include "attitude.h" |
#include "dongfangMath.h" |
#include "commands.h" |
// For scope debugging only! |
#include "rc.h" |
// where our main data flow comes from. |
#include "analog.h" |
#include "configuration.h" |
#include "output.h" |
// Some calculations are performed depending on some stick related things. |
#include "controlMixer.h" |
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
/* |
* Gyro readings, as read from the analog module. It would have been nice to flow |
* them around between the different calculations as a struct or array (doing |
* things functionally without side effects) but this is shorter and probably |
* faster too. |
* The variables are overwritten at each attitude calculation invocation - the values |
* are not preserved or reused. |
*/ |
/* |
* Gyro integrals. These are the rotation angles of the airframe compared to the |
* horizontal plane, yaw relative to yaw at start. |
*/ |
int32_t attitude[3]; |
/* |
* Experiment: Compensating for dynamic-induced gyro biasing. |
*/ |
int16_t driftComp[3] = { 0, 0, 0 }; |
// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
// int16_t dynamicCalCount; |
/************************************************************************ |
* Set inclination angles from the acc. sensor data. |
* If acc. sensors are not used, set to zero. |
* TODO: One could use inverse sine to calculate the angles more |
* accurately, but since: 1) the angles are rather small at times when |
* it makes sense to set the integrals (standing on ground, or flying at |
* constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
* it is hardly worth the trouble. |
************************************************************************/ |
/* |
int32_t getAngleEstimateFromAcc(uint8_t axis) { |
return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; |
} |
*/ |
void setStaticAttitudeAngles(void) { |
attitude[PITCH] = attitude[ROLL] = 0; |
} |
/************************************************************************ |
* Neutral Readings |
************************************************************************/ |
void attitude_setNeutral(void) { |
// Calibrate hardware. |
analog_setNeutral(); |
// reset gyro integrals to acc guessing |
setStaticAttitudeAngles(); |
} |
/************************************************************************ |
* Get sensor data from the analog module, and release the ADC |
* TODO: Ultimately, the analog module could do this (instead of dumping |
* the values into variables). |
* The rate variable end up in a range of about [-1024, 1023]. |
*************************************************************************/ |
void getAnalogData(void) { |
uint8_t axis; |
analog_update(); |
for (axis = PITCH; axis <= YAW; axis++) { |
} |
} |
void integrate(void) { |
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
uint8_t axis; |
/* |
* Yaw |
* Calculate yaw gyro integral (~ to rotation angle) |
* Limit heading proportional to 0 deg to 360 deg |
*/ |
/* |
* Pitch axis integration and range boundary wrap. |
*/ |
for (axis = PITCH; axis <= YAW; axis++) { |
attitude[axis] += gyro_ATT[axis]; |
if (attitude[axis] > OVER180) { |
attitude[axis] -= OVER360; |
} else if (attitude[axis] <= -OVER180) { |
attitude[axis] += OVER360; |
} |
} |
} |
/************************************************************************ |
* Main procedure. |
************************************************************************/ |
void calculateFlightAttitude(void) { |
getAnalogData(); |
integrate(); |
// We are done reading variables from the analog module. |
// Interrupt-driven sensor reading may restart. |
startAnalogConversionCycle(); |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/attitude.h |
---|
0,0 → 1,128 |
/*********************************************************************************/ |
/* Attitude sense system (processing of gyro, accelerometer and altimeter data) */ |
/*********************************************************************************/ |
#ifndef _ATTITUDE_H |
#define _ATTITUDE_H |
#include <inttypes.h> |
#include "analog.h" |
// For debugging only. |
#include "uart0.h" |
/* |
* If you have no acc. sensor or do not want to use it, remove this define. This will cause the |
* acc. sensor to be ignored at attitude calibration. |
*/ |
//#define ATTITUDE_USE_ACC_SENSORS yes |
/* |
* The frequency at which numerical integration takes place. 488 in original code. |
*/ |
#define INTEGRATION_FREQUENCY 488 |
/* |
* Constant for deriving an attitude angle from acceleration measurement. |
* |
* The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef. |
* 1g is (3V * 1024) / (5 * 3V) = 205 counts. The ADC ISR sums 2 acc. samples to each |
* [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g. |
* We approximate a small pitch/roll angle v by assuming that the copter does not accelerate: |
* In this explanation it is assumed that the ADC values are 0 based, and gravity is included. |
* The sine of v is the far side / the hypothenusis: |
* sin v = acc / sqrt(acc^2 + acc_z^2) |
* Using that v is a small angle, and the near side is about equal to the the hypothenusis: |
* sin v ~= acc / acc_z |
* Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410, |
* and sin v ~= v (small angles, in radians): |
* sin v ~= acc / 410 |
* v / 57.3 ~= acc / 410 |
* v ~= acc * 57.3 / 410 |
* acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree. |
* |
* Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3) |
*/ |
#define DEG_ACC_FACTOR 7 |
/* |
* Growth of the integral per degree: |
* The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by |
* HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration. |
* The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable). |
*/ |
#define GYRO_DEG_FACTOR (GYRO_RATE_FACTOR * INTEGRATION_FREQUENCY * GYRO_CORRECTION) |
/* |
* This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value |
* = the factor an acc. sensor should be multiplied by to get the gyro integral |
* value for the same (small) angle. |
* The value is about 200. |
*/ |
//#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR)) |
#define OVER180 ((int32_t)GYRO_DEG_FACTOR * 180) |
#define OVER360 ((int32_t)GYRO_DEG_FACTOR * 360) |
/* |
* Rotation rates |
*/ |
extern int16_t rate_PID[3], rate_ATT[3]; |
extern int16_t differential[3]; |
/* |
* Attitudes calculated by numerical integration of gyro rates |
*/ |
extern int32_t attitude[3]; |
// extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
/* |
* Compass navigation |
*/ |
// extern int16_t compassHeading; |
// extern int16_t compassCourse; |
// extern int16_t compassOffCourse; |
// extern uint8_t compassCalState; |
// extern int32_t yawGyroHeading; |
// extern int16_t yawGyroHeadingInDeg; |
// extern uint8_t updateCompassCourse; |
// extern uint16_t ignoreCompassTimer; |
/* |
* Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements, |
* to help canceling out drift and vibration noise effects. The dynamic offsets themselves |
* can be updated in flight by different ways, for example: |
* - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool |
* - Summing up how much acc. meter correction was done to the gyro integrals over the last n |
* integration, and then adding the sum / n to the dynamic offset |
* - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that |
* - Invent your own... |
*/ |
// extern int16_t dynamicOffset[2], dynamicOffsetYaw; |
/* |
* For NaviCtrl use. |
*/ |
// extern int16_t averageAcc[2], averageAccCount; |
/* |
* Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor). |
* To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right). |
*/ |
void attitude_setNeutral(void); |
/* |
* Experiment. |
*/ |
// void attitude_startDynamicCalibration(void); |
// void attitude_continueDynamicCalibration(void); |
int32_t getAngleEstimateFromAcc(uint8_t axis); |
/* |
* Main routine, called from the flight loop. |
*/ |
void calculateFlightAttitude(void); |
#endif //_ATTITUDE_H |
/branches/dongfang_FC_fixedwing/arduino_atmega328/commands.c |
---|
0,0 → 1,36 |
#include <stdlib.h> |
#include "commands.h" |
#include "controlMixer.h" |
#include "flight.h" |
#include "eeprom.h" |
#include "attitude.h" |
#include "output.h" |
#include "rc.h" |
#ifdef USE_MK3MAG |
// TODO: Kick that all outa here! |
uint8_t compassCalState = 0; |
#endif |
void commands_handleCommands(void) { |
/* |
* Get the current command (start/stop motors, calibrate), if any. |
*/ |
uint8_t command = controlMixer_getCommand(); |
uint8_t repeated = controlMixer_isCommandRepeated(); |
uint8_t argument = controlMixer_getArgument(); |
// TODO! Mode change gadget of some kind. |
if (!isFlying) { |
if (command == COMMAND_GYROCAL && !repeated) { |
// Gyro calinbration, with or without selecting a new parameter-set. |
paramSet_readFromEEProm(1); |
analog_calibrate(); |
attitude_setNeutral(); |
controlMixer_setNeutral(); |
beepNumber(1); |
} else if (command == COMMAND_CHMOD && !repeated) { |
configuration_setFlightParameters(argument); |
} |
} // end !MOTOR_RUN condition. |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/commands.h |
---|
0,0 → 1,11 |
#ifndef _COMMANDS_H |
#define _COMMANDS_H |
#include <inttypes.h> |
#define COMMAND_NONE 0 |
#define COMMAND_GYROCAL 1 |
#define COMMAND_CHMOD 2 |
void commands_handleCommands(void); |
#endif |
/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.c |
---|
0,0 → 1,161 |
#include <util/delay.h> |
#include <stddef.h> |
#include <string.h> |
#include "configuration.h" |
#include "sensors.h" |
#include "rc.h" |
#include "output.h" |
#include "flight.h" |
int16_t variables[VARIABLE_COUNT]; |
ParamSet_t staticParams; |
channelMap_t channelMap; |
IMUConfig_t IMUConfig; |
volatile DynamicParams_t dynamicParams; |
VersionInfo_t versionInfo; |
FlightMode_t currentFlightMode = FLIGHT_MODE_MANUAL; |
// updated by considering airspeed.. |
volatile uint16_t isFlying = 0; |
const MMXLATION XLATIONS[] = { |
{offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255}, |
{offsetof(ParamSet_t, gyroPID[0].P), offsetof(DynamicParams_t, gyroPID[0].P),0,255}, |
{offsetof(ParamSet_t, gyroPID[0].I), offsetof(DynamicParams_t, gyroPID[0].I),0,255}, |
{offsetof(ParamSet_t, gyroPID[0].D), offsetof(DynamicParams_t, gyroPID[0].D),0,255}, |
{offsetof(ParamSet_t, gyroPID[1].P), offsetof(DynamicParams_t, gyroPID[1].P),0,255}, |
{offsetof(ParamSet_t, gyroPID[1].I), offsetof(DynamicParams_t, gyroPID[1].I),0,255}, |
{offsetof(ParamSet_t, gyroPID[1].D), offsetof(DynamicParams_t, gyroPID[1].D),0,255}, |
{offsetof(ParamSet_t, gyroPID[2].P), offsetof(DynamicParams_t, gyroPID[2].P),0,255}, |
{offsetof(ParamSet_t, gyroPID[2].I), offsetof(DynamicParams_t, gyroPID[2].I),0,255}, |
{offsetof(ParamSet_t, gyroPID[2].D), offsetof(DynamicParams_t, gyroPID[2].D),0,255}, |
{offsetof(ParamSet_t, servoConfigurations[0].manualControl), offsetof(DynamicParams_t, servoManualControl[0]),0,255}, |
{offsetof(ParamSet_t, servoConfigurations[1].manualControl), offsetof(DynamicParams_t, servoManualControl[1]),0,255}, |
{offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255}, |
{offsetof(ParamSet_t, outputFlash[1].timing), offsetof(DynamicParams_t, output1Timing),0,255}, |
}; |
uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) { |
uint8_t result; |
if (src>=(256-VARIABLE_COUNT)) result = variables[src-(256-VARIABLE_COUNT)]; |
else result = src; |
if (result < min) result = min; |
else if (result > max) result = max; |
return result; |
} |
void configuration_applyVariablesToParams(void) { |
uint8_t i, src; |
uint8_t* pointerToTgt; |
for(i=0; i<sizeof(XLATIONS)/sizeof(MMXLATION); i++) { |
src = *((uint8_t*)(&staticParams) + XLATIONS[i].sourceIdx); |
pointerToTgt = (uint8_t*)(&dynamicParams) + XLATIONS[i].targetIdx; |
*pointerToTgt = configuration_applyVariableToParam(src, XLATIONS[i].min, XLATIONS[i].max); |
} |
// User parameters are always variable. |
for (i=0; i<sizeof(staticParams.userParams); i++) { |
src = *((uint8_t*)(&staticParams) + offsetof(ParamSet_t, userParams) + i); |
pointerToTgt = (uint8_t*)(&dynamicParams) + offsetof(DynamicParams_t, userParams) + i; |
*pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
} |
} |
void configuration_setFlightParameters(uint8_t newFlightMode) { |
currentFlightMode = newFlightMode; |
flight_updateFlightParametersToFlightMode(); |
} |
// Called after a change in configuration parameters, as a hook for modules to take over changes. |
void configuration_paramSetDidChange(void) { |
// This should be OK to do here as long as we don't change parameters during emergency flight. We don't. |
configuration_setFlightParameters(currentFlightMode); |
// Immediately load changes to output, and also signal the paramset change. |
output_init(); |
} |
void setOtherDefaults(void) { |
// Control |
staticParams.externalControl = 0; |
staticParams.IFactor = 52; |
// Servos |
staticParams.servoCount = 7; |
staticParams.servoManualMaxSpeed = 10; |
for (uint8_t i=0; i<2; i++) { |
staticParams.servoConfigurations[i].manualControl = 128; |
staticParams.servoConfigurations[i].stabilizationFactor = 0; |
staticParams.servoConfigurations[i].minValue = 32; |
staticParams.servoConfigurations[i].maxValue = 224; |
staticParams.servoConfigurations[i].flags = 0; |
} |
// Battery warning and emergency flight |
staticParams.batteryVoltageWarning = 101; // 3.7 each for 3S |
staticParams.emergencyThrottle = 35; |
staticParams.emergencyFlightDuration = 30; |
for (uint8_t i=0; i<3; i++) { |
staticParams.gyroPID[i].P = 80; |
staticParams.gyroPID[i].I = 80; |
staticParams.gyroPID[i].D = 40; |
} |
staticParams.stickIElevator = 80; |
staticParams.stickIAilerons = 120; |
staticParams.stickIRudder = 40; |
// Outputs |
staticParams.outputFlash[0].bitmask = 1; //0b01011111; |
staticParams.outputFlash[0].timing = 15; |
staticParams.outputFlash[1].bitmask = 3; //0b11110011; |
staticParams.outputFlash[1].timing = 15; |
staticParams.outputDebugMask = 8; |
staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | OUTPUTFLAGS_USE_ONBOARD_LEDS; |
} |
/***************************************************/ |
/* Default Values for parameter set 1 */ |
/***************************************************/ |
void paramSet_default(uint8_t setnumber) { |
setOtherDefaults(); |
for (uint8_t i=0; i<8; i++) { |
staticParams.userParams[i] = i; |
} |
staticParams.bitConfig = |
CFG_GYRO_SATURATION_PREVENTION; |
memcpy(staticParams.name, "Default\0", 6); |
} |
void IMUConfig_default(void) { |
IMUConfig.gyroPIDFilterConstant = 1; |
IMUConfig.gyroDFilterConstant = 1; |
IMUConfig.rateTolerance = 120; |
IMUConfig.gyroDWindowLength = 3; |
IMUConfig.gyroQuadrant = 0; |
IMUConfig.imuReversedFlags = 0; |
} |
/***************************************************/ |
/* Default Values for R/C Channels */ |
/***************************************************/ |
void channelMap_default(void) { |
channelMap.channels[CH_ELEVATOR] = 1; |
channelMap.channels[CH_AILERONS] = 0; |
channelMap.channels[CH_THROTTLE] = 2; |
channelMap.channels[CH_RUDDER] = 3; |
channelMap.channels[CH_POTS + 0] = 4; |
channelMap.channels[CH_POTS + 1] = 5; |
channelMap.channels[CH_POTS + 2] = 6; |
channelMap.channels[CH_POTS + 3] = 7; |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.h |
---|
0,0 → 1,228 |
#ifndef _CONFIGURATION_H |
#define _CONFIGURATION_H |
#include <inttypes.h> |
#include <avr/io.h> |
#define MAX_CHANNELS 10 |
#define MAX_MOTORS 12 |
// bitmask for VersionInfo_t.HardwareError[0] |
#define FC_ERROR0_GYRO_PITCH 0x01 |
#define FC_ERROR0_GYRO_ROLL 0x02 |
#define FC_ERROR0_GYRO_YAW 0x04 |
#define FC_ERROR0_ACC_X 0x08 |
#define FC_ERROR0_ACC_Y 0x10 |
#define FC_ERROR0_ACC_Z 0x20 |
#define FC_ERROR0_PRESSURE 0x40 |
#define FC_ERROR1_RES0 0x80 |
// bitmask for VersionInfo_t.HardwareError[1] |
#define FC_ERROR1_I2C 0x01 |
#define FC_ERROR1_BL_MISSING 0x02 |
#define FC_ERROR1_SPI_RX 0x04 |
#define FC_ERROR1_PPM 0x08 |
#define FC_ERROR1_MIXER 0x10 |
#define FC_ERROR1_RES1 0x20 |
#define FC_ERROR1_RES2 0x40 |
#define FC_ERROR1_RES3 0x80 |
typedef struct { |
uint8_t gyroQuadrant; |
uint8_t accQuadrant; |
uint8_t imuReversedFlags; |
uint8_t gyroPIDFilterConstant; |
uint8_t gyroDWindowLength; |
uint8_t gyroDFilterConstant; |
uint8_t accFilterConstant; |
uint8_t zerothOrderCorrection; |
uint8_t rateTolerance; |
uint8_t gyroActivityDamping; |
uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung) |
uint8_t driftCompLimit; // limit for gyrodrift compensation |
} IMUConfig_t; |
extern IMUConfig_t IMUConfig; |
typedef struct { |
uint8_t P; |
uint8_t I; |
uint8_t D; |
} PID_t; |
typedef struct { |
uint8_t P; |
uint8_t I; |
uint8_t D; |
uint8_t iMax; |
} PIDIM_t; |
typedef enum { |
FLIGHT_MODE_NONE, |
FLIGHT_MODE_MANUAL, |
FLIGHT_MODE_RATE, |
FLIGHT_MODE_ANGLES |
} FlightMode_t; |
#define CONTROL_SERVO_REVERSE_ELEVATOR 1 |
#define CONTROL_SERVO_REVERSE_AILERONS 2 |
#define CONTROL_SERVO_REVERSE_RUDDER 4 |
typedef struct { |
uint8_t SWMajor; |
uint8_t SWMinor; |
uint8_t protoMajor; |
uint8_t protoMinor; |
uint8_t SWPatch; |
uint8_t hardwareErrors[5]; |
}__attribute__((packed)) VersionInfo_t; |
extern VersionInfo_t versionInfo; |
typedef struct { |
// IMU stuff: |
PID_t gyroPID[3]; |
// Control |
/* P */uint8_t externalControl; |
// Output and servo |
/*PMM*/uint8_t output0Timing; |
/*PMM*/uint8_t output1Timing; |
uint8_t servoManualControl[2]; |
/* P */uint8_t userParams[8]; |
} DynamicParams_t; |
extern volatile DynamicParams_t dynamicParams; |
typedef struct { |
uint8_t sourceIdx, targetIdx; |
uint8_t min, max; |
} MMXLATION; |
/* |
typedef struct { |
uint8_t sourceIdx, targetIdx; |
} XLATION; |
*/ |
typedef struct { |
uint8_t channels[MAX_CHANNELS]; |
} channelMap_t; |
extern channelMap_t channelMap; |
typedef struct { |
int16_t offsets[3]; |
} sensorOffset_t; |
typedef struct { |
uint8_t manualControl; |
uint8_t stabilizationFactor; |
uint8_t minValue; |
uint8_t maxValue; |
uint8_t flags; |
} servo_t; |
#define SERVO_STABILIZATION_REVERSE 1 |
typedef struct { |
uint8_t bitmask; |
uint8_t timing; |
} output_flash_t; |
// values above 250 representing poti1 to poti4 |
typedef struct { |
// Global bitflags |
uint8_t bitConfig; // see upper defines for bitcoding |
// Control |
PIDIM_t gyroPID[3]; |
uint8_t stickIElevator; |
uint8_t stickIAilerons; |
uint8_t stickIRudder; |
uint8_t externalControl; // for serial Control |
uint8_t IFactor; |
uint8_t batteryVoltageWarning; |
uint8_t emergencyThrottle; |
uint8_t emergencyFlightDuration; |
// Airspeed |
uint8_t airspeedCorrection; |
uint8_t isFlyingThreshold; |
// Servos |
uint8_t controlServosReverse; |
uint8_t servoCount; |
uint8_t servoManualMaxSpeed; |
servo_t servoConfigurations[2]; // [PITCH, ROLL] |
// Outputs |
output_flash_t outputFlash[2]; |
uint8_t outputDebugMask; |
uint8_t outputFlags; |
// User params |
uint8_t userParams[8]; |
// Name |
char name[12]; |
} ParamSet_t; |
extern ParamSet_t staticParams; |
// bit mask for staticParams.bitConfig |
#define CFG_GYRO_SATURATION_PREVENTION (1<<0) |
#define CFG_USE_AIRSPEED_PID (1<<1) |
#define IMU_REVERSE_GYRO_PR (1<<0) |
#define IMU_REVERSE_GYRO_YAW (1<<1) |
#define IMU_REVERSE_ACC_XY (1<<2) |
#define IMU_REVERSE_ACC_Z (1<<3) |
#define ATMEGA644 0 |
#define ATMEGA644P 1 |
#define SYSCLK F_CPU |
// Not really a part of configuration, but LEDs and HW s test are the same. |
#define RED_OFF {PORTD &=~(1<<PORTD6);} |
#define RED_ON {PORTD |= (1<<PORTD6);} |
#define RED_FLASH PORTD ^= (1<<PORTD6) |
#define GRN_OFF {PORTD &=~(1<<PORTD7);} |
#define GRN_ON {PORTD |= (1<<PORTD7);} |
#define GRN_FLASH PORTD ^= (1<<PORTD7) |
// Mixer table |
#define MIX_THROTTLE 0 |
#define MIX_PITCH 1 |
#define MIX_ROLL 2 |
#define MIX_YAW 3 |
#define VARIABLE_COUNT 8 |
extern int16_t variables[VARIABLE_COUNT]; // The "Poti"s. |
extern uint8_t CPUType; |
extern volatile uint16_t isFlying; |
extern FlightMode_t currentFlightMode; |
void IMUConfig_default(void); |
void channelMap_default(void); |
void paramSet_default(uint8_t setnumber); |
void configuration_setFlightParameters(uint8_t newFlightMode); |
void configuration_applyVariablesToParams(void); |
void setCPUType(void); |
// Called after a change in configuration parameters, as a hook for modules to take over changes. |
void configuration_paramSetDidChange(void); |
#endif // _CONFIGURATION_H |
/branches/dongfang_FC_fixedwing/arduino_atmega328/controlMixer.c |
---|
0,0 → 1,157 |
#include <stdlib.h> |
#include "controlMixer.h" |
#include "rc.h" |
#include "externalControl.h" |
#include "failsafeControl.h" |
#include "configuration.h" |
#include "attitude.h" |
#include "commands.h" |
#include "output.h" |
int16_t controls[4] = { 0, 0, 0, 0 }; |
// Internal variables for reading commands made with an R/C stick. |
uint8_t lastCommand = COMMAND_NONE; |
uint8_t lastArgument; |
uint8_t isCommandRepeated = 0; |
uint8_t controlMixer_didReceiveSignal = 0; |
/* |
* This could be expanded to take arguments from ohter sources than the RC |
* (read: Custom MK RC project) |
*/ |
uint8_t controlMixer_getArgument(void) { |
return lastArgument; |
} |
/* |
* This could be expanded to take calibrate / start / stop commands from ohter sources |
* than the R/C (read: Custom MK R/C project) |
*/ |
uint8_t controlMixer_getCommand(void) { |
return lastCommand; |
} |
uint8_t controlMixer_isCommandRepeated(void) { |
return isCommandRepeated; |
} |
void controlMixer_setNeutral() { |
for (uint8_t i=0; i<VARIABLE_COUNT; i++) { |
variables[i] = RC_getVariable(i); |
} |
EC_setNeutral(); |
FC_setNeutral(); // FC is FailsafeControl, not FlightCtrl. |
} |
/* |
* Update potentiometer values with limited slew rate. Could be made faster if desired. |
* TODO: It assumes R/C as source. Not necessarily true. |
*/ |
void controlMixer_updateVariables(void) { |
uint8_t i; |
int16_t targetvalue; |
for (i=0; i < VARIABLE_COUNT; i++) { |
targetvalue = RC_getVariable(i); |
if (targetvalue < 0) |
targetvalue = 0; |
if (variables[i] < targetvalue && variables[i] < 255) |
variables[i]++; |
else if (variables[i] > 0 && variables[i] > targetvalue) |
variables[i]--; |
} |
} |
uint8_t controlMixer_getSignalQuality(void) { |
uint8_t rcQ = RC_getSignalQuality(); |
uint8_t ecQ = EC_getSignalQuality(); |
// This needs not be the only correct solution... |
return rcQ > ecQ ? rcQ : ecQ; |
} |
/* |
void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) { |
int16_t tmp = controls[index]; |
controls[index] = newValue; |
tmp -= newValue; |
tmp /= 2; |
tmp = tmp * tmp; |
// tmp += (newValue >= 0) ? newValue : -newValue; |
/ * |
if (controlActivity + (uint16_t)tmp >= controlActivity) |
controlActivity += tmp; |
else controlActivity = 0xffff; |
* / |
if (controlActivity + (uint16_t)tmp < 0x8000) |
controlActivity += tmp; |
} |
#define CADAMPING 10 |
void dampenControlActivity(void) { |
uint32_t tmp = controlActivity; |
tmp *= ((1<<CADAMPING)-1); |
tmp >>= CADAMPING; |
controlActivity = tmp; |
} |
*/ |
/* |
* Update the variables indicating stick position from the sum of R/C, GPS and external control |
* and whatever other controls we invented in the meantime... |
* Update variables. |
* Decode commands but do not execute them. |
*/ |
void controlMixer_periodicTask(void) { |
int16_t tempPRYT[4] = { 0, 0, 0, 0 }; |
// Decode commands. |
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE; |
uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE; |
// Update variables ("potis"). |
if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
controlMixer_updateVariables(); |
controlMixer_didReceiveSignal = 1; |
} else { // Signal is not OK |
// Could handle switch to emergency flight here. |
// throttle is handled elsewhere. |
} |
if (rcCommand != COMMAND_NONE) { |
isCommandRepeated = (lastCommand == rcCommand); |
lastCommand = rcCommand; |
lastArgument = RC_getArgument(); |
} else if (ecCommand != COMMAND_NONE) { |
isCommandRepeated = (lastCommand == ecCommand); |
lastCommand = ecCommand; |
lastArgument = EC_getArgument(); |
} else { |
// Both sources have no command, or one or both are out. |
// Just set to false. There is no reason to check if the none-command was repeated anyway. |
isCommandRepeated = 0; |
lastCommand = COMMAND_NONE; |
} |
// This will init the values (not just add to them). |
RC_periodicTaskAndPRYT(tempPRYT); |
// Add external control to RC |
EC_periodicTaskAndPRYT(tempPRYT); |
FC_periodicTaskAndPRYT(tempPRYT); |
// Commit results to global variable and also measure control activity. |
controls[CONTROL_THROTTLE] = tempPRYT[CONTROL_THROTTLE]; |
controls[CONTROL_ELEVATOR] = tempPRYT[CONTROL_ELEVATOR]; |
controls[CONTROL_AILERONS] = tempPRYT[CONTROL_AILERONS]; |
controls[CONTROL_RUDDER] = tempPRYT[CONTROL_RUDDER]; |
// dampenControlActivity(); |
// We can safely do this even with a bad signal - the variables will not have been updated then. |
configuration_applyVariablesToParams(); |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/controlMixer.h |
---|
0,0 → 1,119 |
#ifndef _CONTROLMIXER_H |
#define _CONTROLMIXER_H |
#include <inttypes.h> |
/* |
* An attempt to define a generic control. That could be an R/C receiver, an external control |
* (serial over Bluetooth, Wi232, XBee, whatever) or the NaviCtrl. |
* This resembles somewhat an object-oriented class definition (except that there are no virtuals). |
* The idea is that the combination of different control inputs, of the way they superimpose upon |
* each other, the priorities between them and the behavior in case that one fails is simplified, |
* and all in one place. |
*/ |
/* |
* Signal qualities, used to determine the availability of a control. |
* NO_SIGNAL means there was never a signal. SIGNAL_LOST that there was a signal, but it was lost. |
* SIGNAL_BAD is too bad for flight. This is the hysteresis range for deciding whether to engage |
* or disengage emergency landing. |
* SIGNAL_OK means the signal is usable for flight. |
* SIGNAL_GOOD means the signal can also be used for setting parameters. |
*/ |
#define NO_SIGNAL 0 |
#define SIGNAL_LOST 1 |
#define SIGNAL_BAD 2 |
#define SIGNAL_OK 3 |
#define SIGNAL_GOOD 4 |
/* |
* The PRYT arrays |
*/ |
#define CONTROL_ELEVATOR 0 |
#define CONTROL_AILERONS 1 |
#define CONTROL_RUDDER 2 |
#define CONTROL_THROTTLE 3 |
/* |
* Looping flags. |
* LOOPING_UP || LOOPING_DOWN <=> LOOPING_PITCH_AXIS |
* LOOPING_LEFT || LOOPING_RIGHT <=> LOOPING_ROLL_AXIS |
*/ |
#define LOOPING_UP (1<<0) |
#define LOOPING_DOWN (1<<1) |
#define LOOPING_LEFT (1<<2) |
#define LOOPING_RIGHT (1<<3) |
#define LOOPING_PITCH_AXIS (1<<4) |
#define LOOPING_ROLL_AXIS (1<<5) |
/* |
* This is only relevant for "abstract controls" ie. all control sources have the |
* same interface. This struct of code pointers is used like an abstract class |
* definition from object-oriented languages, and all control input implementations |
* will declare an instance of the stuct (=implementation of the abstract class). |
*/ |
typedef struct { |
/* Get the pitch input in the nominal range [-STICK_RANGE, STICK_RANGE]. */ |
int16_t(*getPitch)(void); |
/* Get the roll input in the nominal range [-STICK_RANGE, STICK_RANGE]. */ |
int16_t(*getRoll)(void); |
/* Get the yaw input in the nominal range [-STICK_RANGE, STICK_RANGE]. */ |
int16_t(*getYaw)(void); |
/* Get the throttle input in the nominal range [0, THROTTLE_RANGE]. */ |
uint16_t(*getThrottle)(void); |
/* Signal quality, by the above SIGNAL_... definitions. */ |
uint8_t (*getSignalQuality)(void); |
/* Calibrate sticks to their center positions (only relevant for R/C, really) */ |
void (*calibrate)(void); |
} t_control; |
/* |
* Our output. |
*/ |
extern int16_t controls[4]; |
extern uint16_t controlActivity; |
//extern uint16_t maxControl[2]; |
void controlMixer_initVariables(void); |
//void controlMixer_updateVariables(void); |
void controlMixer_setNeutral(void); |
/* |
* Update the exported variables. Called at every flight control cycle. |
*/ |
void controlMixer_periodicTask(void); |
/* |
* Get the current command. See the COMMAND_.... define's |
*/ |
uint8_t controlMixer_getCommand(void); |
void controlMixer_performCalibrationCommands(uint8_t command); |
uint8_t controlMixer_getSignalQuality(void); |
extern uint8_t controlMixer_didReceiveSignal; |
/* |
* Gets the argument for the current command (a number). |
* |
* Stick position to argument values (for stick control): |
* 2--3--4 |
* | | + |
* 1 9 5 ^ 0 |
* | | | |
* 8--7--6 |
* |
* + <-- |
* 0 |
* |
* Not in any of these positions: 0 |
*/ |
uint8_t controlMixer_getArgument(void); |
uint8_t controlMixer_isCommandRepeated(void); |
#endif |
/branches/dongfang_FC_fixedwing/arduino_atmega328/dongfangMath.c |
---|
0,0 → 1,239 |
#include "dongfangMath.h" |
#include <inttypes.h> |
#include <avr/pgmspace.h> |
// For scope debugging only! |
#include "output.h" |
const int16_t SIN_TABLE[] PROGMEM = { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.01745240643728351 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.03489949670250097 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.05233595624294383 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.0697564737441253 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.08715574274765817 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.10452846326765346 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.12186934340514748 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.13917310096006544 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.15643446504023087 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.17364817766693033 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.1908089953765448 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.20791169081775931 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.224951054343865 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.24192189559966773 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.25881904510252074 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.27563735581699916 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.29237170472273677 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3090169943749474 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.32556815445715664 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3420201433256687 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.35836794954530027 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.374606593415912 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3907311284892737 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.40673664307580015 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.42261826174069944 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4383711467890774 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.45399049973954675 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4694715627858908 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.48480962024633706 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.49999999999999994 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5150380749100542 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5299192642332049 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5446390350150271 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5591929034707469 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.573576436351046 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5877852522924731 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6018150231520483 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6156614753256582 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6293203910498374 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6427876096865393 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6560590289905072 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6691306063588582 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6819983600624985 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6946583704589973 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7071067811865475 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7193398003386511 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7313537016191705 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7431448254773942 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.754709580222772 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.766044443118978 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7771459614569708 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.788010753606722 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7986355100472928 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8090169943749475 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8191520442889918 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8290375725550417 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8386705679454239 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8480480961564261 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8571673007021122 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8660254037844386 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8746197071393957 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8829475928589269 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8910065241883678 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.898794046299167 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9063077870366499 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9135454576426009 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9205048534524403 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9271838545667874 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9335804264972017 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9396926207859083 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9455185755993167 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9510565162951535 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9563047559630354 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9612616959383189 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9659258262890683 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9702957262759965 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9743700647852352 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9781476007338056 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.981627183447664 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.984807753012208 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9876883405951378 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9902680687415703 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.992546151641322 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9945218953682733 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9961946980917455 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9975640502598242 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9986295347545738 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9993908270190958 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (MATH_UNIT_FACTOR) + 0.5 }; |
const int16_t TAN_TABLE[] PROGMEM |
= { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.017455064928217585 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.03492076949174773 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.0524077792830412 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.06992681194351041 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.08748866352592401 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.10510423526567646 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.1227845609029046 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.14054083470239145 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.15838444032453627 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.17632698070846498 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.19438030913771848 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.2125565616700221 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.23086819112556312 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.24932800284318068 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.2679491924311227 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.2867453857588079 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3057306814586604 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3249196962329063 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3443276132896652 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.36397023426620234 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.3838640350354158 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4040262258351568 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4244748162096047 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4452286853085361 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.4663076581549986 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.48773258856586144 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5095254494944288 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5317094316614788 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.554309051452769 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.5773502691896257 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6008606190275604 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6248693519093275 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6494075931975106 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.6745085168424267 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7002075382097097 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7265425280053608 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7535540501027942 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.7812856265067173 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.809784033195007 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8390996311772799 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.8692867378162265 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9004040442978399 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9325150861376615 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9656887748070739 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9999999999999999 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.0355303137905694 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.0723687100246826 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.1106125148291928 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.1503684072210094 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.19175359259421 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.234897156535051 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.2799416321930788 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.3270448216204098 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.3763819204711734 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.4281480067421144 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.4825609685127403 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.5398649638145825 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.6003345290410504 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.6642794823505174 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.7320508075688767 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.8040477552714236 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.8807264653463318 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (1.9626105055051504 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.050303841579296 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.1445069205095586 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.2460367739042164 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.355852365823752 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.4750868534162964 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.6050890646938005 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.7474774194546216 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (2.904210877675822 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (3.0776835371752527 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (3.2708526184841404 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (3.4874144438409087 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (3.7320508075688776 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (4.010780933535842 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (4.331475874284157 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (4.704630109478451 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (5.144554015970307 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (5.671281819617707 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (6.313751514675041 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (7.115369722384195 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (8.144346427974593 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (9.514364454222587 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (11.430052302761348 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (14.300666256711896 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (19.08113668772816 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (28.636253282915515 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (32767) }; |
int16_t sin_360(int16_t arg) { |
int8_t sign; |
int16_t result; |
arg %= 360; |
if (arg < 0) { |
arg = -arg; |
sign = -1; |
} else { |
sign = 1; |
} |
if (arg > 180) { |
arg = 360 - arg; |
sign = -sign; |
} |
if (arg > 90) { |
arg = 180 - arg; |
} |
result = pgm_read_word(&SIN_TABLE[(uint8_t) arg]); |
return (sign == 1) ? result : -result; |
} |
int16_t cos_360(int16_t arg) { |
return sin_360(arg + 90); |
} |
int16_t tan_360(int16_t arg) { |
int8_t sign = 1; |
int16_t result; |
if (arg < 0) { |
arg = -arg; |
sign = -1; |
} |
if (arg >= 90) { |
arg = 180 - arg; |
sign = -sign; |
} |
result = pgm_read_word(&TAN_TABLE[(uint8_t) arg]); |
return (sign == 1) ? result : -result; |
} |
void intervalWrap(int32_t *number, int32_t limit) { |
if (*number >= limit) { |
*number -= limit; // 360 deg. wrap |
} else if (*number < 0) { |
*number += limit; |
} |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/dongfangMath.h |
---|
0,0 → 1,24 |
#include<inttypes.h> |
#include "attitude.h" |
/* |
* Angular unit scaling: Number of units per degree |
*/ |
#define MATH_DRG_FACTOR GYRO_DEG_FACTOR |
/* |
* Fix-point decimal scaling: Number of units for 1 (so if sin(something) |
* returns UNIT_FACTOR * 0.8, the result is to be understood as 0.8) |
* a * sin(b) = (a * int_sin(b * DRG_FACTOR)) / UNIT_FACTOR |
*/ |
//#define MATH_UNIT_FACTOR 8192 |
// Changed: We want to be able to multiply 2 sines/cosines and still stay comfortably (factor 100) within 31 bits. |
// 4096 = 12 bits, square = 24 bits, 7 bits to spare. |
#define LOG_MATH_UNIT_FACTOR 12 |
#define MATH_UNIT_FACTOR (1L<<LOG_MATH_UNIT_FACTOR) |
int16_t sin_360(int16_t arg); |
int16_t cos_360(int16_t arg); |
int16_t tan_360(int16_t arg); |
void intervalWrap(int32_t *number, int32_t limit); |
/branches/dongfang_FC_fixedwing/arduino_atmega328/eeprom.c |
---|
0,0 → 1,179 |
#ifndef EEMEM |
#define EEMEM __attribute__ ((section (".eeprom"))) |
#endif |
#include "eeprom.h" |
#include "printf_P.h" |
#include "output.h" |
#include <avr/wdt.h> |
#include <avr/eeprom.h> |
// byte array in eeprom |
uint8_t EEPromArray[E2END + 1] EEMEM; |
/***************************************************/ |
/* Read Parameter from EEPROM as byte */ |
/***************************************************/ |
uint8_t getParamByte(uint16_t param_id) { |
return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]); |
} |
/***************************************************/ |
/* Write Parameter to EEPROM as byte */ |
/***************************************************/ |
void setParamByte(uint16_t param_id, uint8_t value) { |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
} |
/***************************************************/ |
/* Read Parameter from EEPROM as word */ |
/***************************************************/ |
/* |
uint16_t getParamWord(uint16_t param_id) { |
return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN |
+ param_id]); |
} |
*/ |
/***************************************************/ |
/* Write Parameter to EEPROM as word */ |
/***************************************************/ |
/* |
void setParamWord(uint16_t param_id, uint16_t value) { |
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
} |
*/ |
uint16_t CRC16(uint8_t* data, uint16_t length) { |
uint16_t crc = 0; |
for (uint16_t i=0; i<length; i++) { |
crc = (uint8_t)(crc >> 8) | (crc << 8); |
crc ^= data[i]; |
crc ^= (uint8_t)(crc & 0xff) >> 4; |
crc ^= (crc << 8) << 4; |
crc ^= ((crc & 0xff) << 4) << 1; |
} |
return crc; |
} |
// offset is where the checksum is stored, offset+1 is the revision number, and offset+2... are the data. |
// length is the length of the pure data not including checksum and revision number. |
void writeChecksummedBlock(uint8_t revisionNumber, uint8_t* data, uint16_t offset, uint16_t length) { |
uint16_t CRC = CRC16(data, length); |
eeprom_write_byte(&EEPromArray[offset], CRC&0xff); |
eeprom_write_byte(&EEPromArray[offset+1], CRC>>8); |
eeprom_write_byte(&EEPromArray[offset+2], revisionNumber); |
eeprom_write_block(data, &EEPromArray[offset+3], length); |
} |
// offset is where the checksum is stored, offset+1 is the revision number, and offset+2... are the data. |
// length is the length of the pure data not including checksum and revision number. |
uint8_t readChecksummedBlock(uint8_t revisionNumber, uint8_t* target, uint16_t offset, uint16_t length) { |
uint16_t CRCRead = eeprom_read_byte(&EEPromArray[offset]) | (eeprom_read_byte(&EEPromArray[offset+1])<<8); |
uint8_t revisionNumberRead = eeprom_read_byte(&EEPromArray[offset+2]); |
eeprom_read_block(target, &EEPromArray[offset+3], length); |
uint16_t CRCCalculated = CRC16(target, length); |
uint8_t CRCError = (CRCRead != CRCCalculated); |
uint8_t revisionMismatch = (revisionNumber != revisionNumberRead); |
if (CRCError && revisionMismatch) printf("\n\rEEPROM CRC error and revision mismatch; "); |
else if (CRCError) printf("\n\rEEPROM CRC error; "); |
else if (revisionMismatch) printf("\n\rEEPROM revision mismatch; "); |
return (CRCError || revisionMismatch); |
} |
/***************************************************/ |
/* Read Parameter Set from EEPROM */ |
/***************************************************/ |
// setnumber [1..5] |
uint8_t paramSet_readFromEEProm(uint8_t setnumber) { |
uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*(sizeof(ParamSet_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD); |
uint8_t result = readChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, offset, sizeof(ParamSet_t)); |
configuration_paramSetDidChange(); |
return result; |
} |
/***************************************************/ |
/* Write Parameter Set to EEPROM */ |
/***************************************************/ |
void paramSet_writeToEEProm(uint8_t setnumber) { |
uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*(sizeof(ParamSet_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD); |
writeChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, offset, sizeof(ParamSet_t)); |
// set this parameter set to active set |
} |
void paramSet_readOrDefault() { |
// parameter version check |
if (paramSet_readFromEEProm(1)) { |
// if version check faild |
printf("\n\rwriting default parameter sets"); |
for (uint8_t i=5; i>0; i--) { |
paramSet_default(i); |
paramSet_writeToEEProm(i); |
} |
// default-Setting is parameter set 1 |
paramSet_readFromEEProm(1); |
// For some strange reason, the read will have no effect. |
// Lets reset... |
// wdt_enable(WDTO_500MS); |
} |
printf("\n\r\rUsing Parameter Set %d", 1); |
} |
/***************************************************/ |
/* Read IMU Config from EEPROM */ |
/***************************************************/ |
uint8_t IMUConfig_readFromEEprom(void) { |
return readChecksummedBlock(IMUCONFIG_REVISION, (uint8_t*)&IMUConfig, EEPROM_ADR_IMU_CONFIG, sizeof(IMUConfig_t)); |
} |
/***************************************************/ |
/* Write IMU Config to EEPROM */ |
/***************************************************/ |
void IMUConfig_writeToEEprom(void) { |
writeChecksummedBlock(IMUCONFIG_REVISION, (uint8_t*)&IMUConfig, EEPROM_ADR_IMU_CONFIG, sizeof(IMUConfig_t)); |
} |
void IMUConfig_readOrDefault(void) { |
if(IMUConfig_readFromEEprom()) { |
printf("\n\rwriting default IMU config"); |
IMUConfig_default(); |
IMUConfig_writeToEEprom(); |
} |
} |
/***************************************************/ |
/* ChannelMap */ |
/***************************************************/ |
void channelMap_writeToEEProm(void) { |
writeChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t)); |
} |
void channelMap_readOrDefault(void) { |
if (readChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t))) { |
printf("\n\rwriting default channel map"); |
channelMap_default(); |
channelMap_writeToEEProm(); |
wdt_enable(WDTO_500MS); |
} |
} |
/***************************************************/ |
/* Sensor offsets */ |
/***************************************************/ |
uint8_t gyroOffset_readFromEEProm(void) { |
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroOffset, EEPROM_ADR_GYROOFFSET, sizeof(sensorOffset_t)); |
} |
void gyroOffset_writeToEEProm(void) { |
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroOffset, EEPROM_ADR_GYROOFFSET, sizeof(sensorOffset_t)); |
} |
uint8_t airpressureOffset_readFromEEProm(void) { |
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&airpressureOffset, EEPROM_ADR_AIRSPEEDOFFSET, sizeof(uint16_t)); |
} |
void airpressureOffset_writeToEEProm(void) { |
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&airpressureOffset, EEPROM_ADR_AIRSPEEDOFFSET, sizeof(uint16_t)); |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/eeprom.h |
---|
0,0 → 1,58 |
#ifndef _EEPROM_H |
#define _EEPROM_H |
#include <inttypes.h> |
#include "configuration.h" |
#include "analog.h" |
#define EEPROM_ADR_PARAM_BEGIN 0 |
#define EEPROM_CHECKSUMMED_BLOCK_OVERHEAD 3 |
#define PID_ACTIVE_SET 0 // byte |
//#define EEPROM_ADR_ACCOFFSET 1 |
//#define EEPROM_ADR_GYROOFFSET (EEPROM_ADR_ACCOFFSET+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
//#define EEPROM_ADR_GYROAMPLIFIER (EEPROM_ADR_GYROOFFSET+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
//#define EEPROM_ADR_CHANNELMAP (EEPROM_ADR_GYROAMPLIFIER+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
//#define EEPROM_ADR_PARAMSET_BEGIN (EEPROM_ADR_MIXER_TABLE+sizeof(mixerMatrix_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
#define EEPROM_ADR_AIRSPEEDOFFSET 16 |
#define EEPROM_ADR_GYROOFFSET 32 |
#define EEPROM_ADR_GYROAMPLIFIER 48 |
#define EEPROM_ADR_CHANNELMAP 64 |
#define EEPROM_ADR_IMU_CONFIG 100 |
#define EEPROM_ADR_PARAMSET_BEGIN 256 |
#define CHANNELMAP_REVISION 0 |
#define EEPARAM_REVISION 4 |
#define EEMIXER_REVISION 0 |
#define SENSOROFFSET_REVISION 0 |
#define IMUCONFIG_REVISION 0 |
void paramSet_readOrDefault(void); |
void channelMap_readOrDefault(void); |
void IMUConfig_readOrDefault(void); |
void IMUConfig_writeToEEprom(void); |
uint8_t paramSet_readFromEEProm(uint8_t setnumber); |
void paramSet_writeToEEProm(uint8_t setnumber); |
void channelMap_writeToEEProm(void); |
uint8_t gyroOffset_readFromEEProm(void); |
void gyroOffset_writeToEEProm(void); |
uint8_t airpressureOffset_readFromEEProm(void); |
void airpressureOffset_writeToEEProm(void); |
//uint8_t accOffset_readFromEEProm(void); |
//void accOffset_writeToEEProm(void); |
uint8_t getParamByte(uint16_t param_id); |
void setParamByte(uint16_t param_id, uint8_t value); |
// We have only a single param set here. |
// uint8_t getActiveParamSet(void); |
// void setActiveParamSet(uint8_t setnumber); |
#endif //_EEPROM_H |
/branches/dongfang_FC_fixedwing/arduino_atmega328/externalControl.c |
---|
0,0 → 1,58 |
#include "externalControl.h" |
#include "configuration.h" |
#include "controlMixer.h" |
ExternalControl_t externalControl; |
uint8_t externalControlActive = 0; |
void EC_setNeutral(void) { |
// if necessary. From main.c. |
externalControl.config = 0; |
externalControl.pitch = 0; |
externalControl.roll = 0; |
externalControl.yaw = 0; |
externalControl.throttle = 0; |
// From main.c. What does it do?? |
externalControl.digital[0] = 0x55; |
} |
void EC_periodicTaskAndPRYT(int16_t* PRYT) { |
if (externalControlActive) { |
externalControlActive--; |
PRYT[CONTROL_ELEVATOR] += externalControl.pitch * 8; |
PRYT[CONTROL_AILERONS] += externalControl.roll * 8; |
PRYT[CONTROL_THROTTLE] += externalControl.throttle * 8; |
PRYT[CONTROL_RUDDER] += externalControl.yaw * 8; |
} |
} |
uint8_t EC_getArgument(void) { |
return externalControl.config; |
} |
uint8_t EC_getCommand(void) { |
return externalControl.free; |
} |
// not implemented. |
int16_t EC_getVariable(uint8_t varNum) { |
return 0; |
} |
uint8_t EC_getSignalQuality(void) { |
if (externalControlActive > 40) |
// Configured and heard from recently |
return SIGNAL_GOOD; |
if (externalControlActive) |
// Configured and heard from |
return SIGNAL_OK; |
if (!(externalControl.config & 0x01 && dynamicParams.externalControl > 128)) |
// External control is not even configured. |
return NO_SIGNAL; |
// Configured but expired. |
return SIGNAL_LOST; |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/externalControl.h |
---|
0,0 → 1,31 |
// Or does this simply belong in uart0.h?? |
#ifndef _EXTERNALCONTROL_H |
#define _EXTERNALCONTROL_H |
#include<inttypes.h> |
typedef struct { |
uint8_t digital[2]; |
uint8_t remoteButtons; |
int8_t pitch; |
int8_t roll; |
int8_t yaw; |
uint8_t throttle; |
int8_t height; |
uint8_t free; // Let's use that for commands now. |
uint8_t frame; |
uint8_t config; // Let's use that for arguemnts. |
}__attribute__((packed)) ExternalControl_t; |
extern ExternalControl_t externalControl; |
extern uint8_t externalControlActive; |
void EC_periodicTaskAndPRYT(int16_t* PRYT); |
uint8_t EC_getArgument(void); |
uint8_t EC_getCommand(void); |
int16_t EC_getVariable(uint8_t varNum); |
void EC_calibrate(void); |
uint8_t EC_getSignalQuality(void); |
void EC_setNeutral(void); |
#endif |
/branches/dongfang_FC_fixedwing/arduino_atmega328/failsafeControl.c |
---|
0,0 → 1,16 |
#include "controlMixer.h" |
#include "timer0.h" |
#include "configuration.h" |
#include "flight.h" |
#include "output.h" |
void FC_periodicTaskAndPRYT(uint16_t* PRYT) { |
if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
if (controlMixer_didReceiveSignal) { |
beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss. |
} |
} |
} |
void FC_setNeutral(void) { |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/failsafeControl.h |
---|
0,0 → 1,3 |
#include <inttypes.h> |
void FC_setNeutral(void); |
void FC_periodicTaskAndPRYT(int16_t* PRYT); |
/branches/dongfang_FC_fixedwing/arduino_atmega328/flight.c |
---|
0,0 → 1,231 |
#include <stdlib.h> |
#include <avr/io.h> |
#include "eeprom.h" |
#include "flight.h" |
#include "output.h" |
// Necessary for external control and motor test |
#include "uart0.h" |
#include "timer2.h" |
#include "analog.h" |
#include "attitude.h" |
#include "controlMixer.h" |
#include "configuration.h" |
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
/* |
* target-directions integrals. |
*/ |
int32_t target[3]; |
/* |
* Error integrals. |
*/ |
int32_t error[3]; |
uint8_t reverse[3]; |
int32_t maxError[3]; |
int32_t IPart[3] = { 0, 0, 0 }; |
PID_t airspeedPID[3]; |
int16_t controlServos[NUM_CONTROL_SERVOS]; |
/************************************************************************/ |
/* Neutral Readings */ |
/************************************************************************/ |
#define CONTROL_CONFIG_SCALE 10 |
void flight_setGround(void) { |
IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0; |
target[PITCH] = attitude[PITCH]; |
target[ROLL] = attitude[ROLL]; |
target[YAW] = attitude[YAW]; |
} |
void flight_updateFlightParametersToFlightMode(void) { |
debugOut.analog[16] = currentFlightMode; |
reverse[PITCH] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_ELEVATOR; |
reverse[ROLL] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_AILERONS; |
reverse[YAW] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_RUDDER; |
// At a switch to angles, we want to kill errors first. |
// This should be triggered only once per mode change! |
if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
target[PITCH] = attitude[PITCH]; |
target[ROLL] = attitude[ROLL]; |
target[YAW] = attitude[YAW]; |
} |
for (uint8_t axis=0; axis<3; axis++) { |
maxError[axis] = (int32_t)staticParams.gyroPID[axis].iMax * GYRO_DEG_FACTOR; |
} |
} |
// Normal at airspeed = 10. |
uint8_t calcAirspeedPID(uint8_t pid) { |
if (staticParams.bitConfig & CFG_USE_AIRSPEED_PID) { |
return pid; |
} |
uint16_t result = (pid * 10) / airspeedVelocity; |
if (result > 240 || airspeedVelocity == 0) { |
result = 240; |
} |
return result; |
} |
void setAirspeedPIDs(void) { |
for (uint8_t axis = 0; axis<3; axis++) { |
airspeedPID[axis].P = calcAirspeedPID(dynamicParams.gyroPID[axis].P); |
airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be??? |
airspeedPID[axis].D = dynamicParams.gyroPID[axis].D; |
} |
} |
/************************************************************************/ |
/* Main Flight Control */ |
/************************************************************************/ |
void flight_control(void) { |
// Mixer Fractions that are combined for Motor Control |
int16_t term[4]; |
// PID controller variables |
int16_t PDPart[3]; |
static int8_t debugDataTimer = 1; |
// High resolution motor values for smoothing of PID motor outputs |
// static int16_t outputFilters[MAX_OUTPUTS]; |
uint8_t axis; |
setAirspeedPIDs(); |
term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
// These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
target[PITCH] += (controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 6; |
target[ROLL] += (controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 6; |
target[YAW] += (controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 6; |
for (axis = PITCH; axis <= YAW; axis++) { |
if (target[axis] > OVER180) { |
target[axis] -= OVER360; |
} else if (target[axis] <= -OVER180) { |
target[axis] += OVER360; |
} |
if (reverse[axis]) |
error[axis] = attitude[axis] + target[axis]; |
else |
error[axis] = attitude[axis] - target[axis]; |
if (error[axis] > maxError[axis]) { |
error[axis] = maxError[axis]; |
} else if (error[axis] < -maxError[axis]) { |
error[axis] =- maxError[axis]; |
} |
#define LOG_P_SCALE 6 |
#define LOG_I_SCALE 6 |
#define LOG_D_SCALE 4 |
/************************************************************************/ |
/* Calculate control feedback from angle (gyro integral) */ |
/* and angular velocity (gyro signal) */ |
/************************************************************************/ |
if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode |
== FLIGHT_MODE_RATE) { |
PDPart[axis] = (((int32_t) gyro_PID[axis] |
* (int16_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
+ ((gyroD[axis] * (int16_t) airspeedPID[axis].D) |
>> LOG_D_SCALE); |
if (reverse[axis]) |
PDPart[axis] = -PDPart[axis]; |
} else { |
PDPart[axis] = 0; |
} |
if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
int16_t anglePart = (int32_t)( |
error[axis] * (int32_t) airspeedPID[axis].I) |
>> LOG_I_SCALE; |
if (reverse[axis]) |
PDPart[axis] += anglePart; |
else |
PDPart[axis] -= anglePart; |
} |
// Add I parts here... these are integrated errors. |
// When an error wraps, actually its I part should be negated or something... |
term[axis] = controls[axis] + PDPart[axis] + IPart[axis]; |
} |
debugOut.analog[12] = term[PITCH]; |
debugOut.analog[13] = term[ROLL]; |
debugOut.analog[14] = term[YAW]; |
debugOut.analog[15] = term[THROTTLE]; |
for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) { |
int16_t tmp; |
if (servoTestActive) { |
controlServos[i] = ((int16_t) servoTest[i] - 128) * 4; |
} else { |
// Follow the normal order of servos: Ailerons, elevator, throttle, rudder. |
switch (i) { |
case 0: |
tmp = term[ROLL]; |
break; |
case 1: |
tmp = term[PITCH]; |
break; |
case 2: |
tmp = term[THROTTLE]; |
break; |
case 3: |
tmp = term[YAW]; |
break; |
default: |
tmp = 0; |
} |
// These are all signed and in the same units as the RC stuff in rc.c. |
controlServos[i] = tmp; |
} |
} |
calculateControlServoValues(); |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Debugging |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if (!(--debugDataTimer)) { |
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
debugOut.analog[0] = gyro_PID[PITCH]; // in 0.1 deg |
debugOut.analog[1] = gyro_PID[ROLL]; // in 0.1 deg |
debugOut.analog[2] = gyro_PID[YAW]; |
debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10); |
debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10); |
debugOut.analog[12] = term[PITCH]; |
debugOut.analog[13] = term[ROLL]; |
debugOut.analog[14] = term[YAW]; |
//DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
//DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
//debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg |
//debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg |
} |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/flight.h |
---|
0,0 → 1,25 |
/*********************************************************************************/ |
/* Flight Control */ |
/*********************************************************************************/ |
#ifndef _FLIGHT_H |
#define _FLIGHT_H |
#include <inttypes.h> |
#include "analog.h" |
#include "configuration.h" |
#define PITCH 0 |
#define ROLL 1 |
#define YAW 2 |
#define THROTTLE 3 |
#define NUM_CONTROL_SERVOS 4 |
extern int16_t controlServos[NUM_CONTROL_SERVOS]; |
void flight_control(void); |
void flight_setNeutral(void); |
void flight_updateFlightParametersToFlightMode(void); |
#endif //_FLIGHT_H |
/branches/dongfang_FC_fixedwing/arduino_atmega328/invenSense.c |
---|
0,0 → 1,41 |
#include "timer0.h" |
#include "configuration.h" |
#include <avr/io.h> |
/* |
* Configuration for my prototype board with InvenSense gyros. |
* The FC 1.3 board is installed upside down, therefore Z acc is reversed but not roll. |
*/ |
// The special auto-zero feature of this gyro needs a port. |
#define AUTOZERO_PORT PORTD |
#define AUTOZERO_DDR DDRD |
#define AUTOZERO_BIT 5 |
void gyro_calibrate(void) { |
// If port not already set to output and high, do it. |
if (!(AUTOZERO_DDR & (1 << AUTOZERO_BIT)) || !(AUTOZERO_PORT & (1 << AUTOZERO_BIT))) { |
AUTOZERO_PORT |= (1 << AUTOZERO_BIT); |
AUTOZERO_DDR |= (1 << AUTOZERO_BIT); |
delay_ms(100); |
} |
// Make a pulse on the auto-zero output line. |
AUTOZERO_PORT &= ~(1 << AUTOZERO_BIT); |
delay_ms(1); |
AUTOZERO_PORT |= (1 << AUTOZERO_BIT); |
// Delay_ms(10); |
delay_ms_with_adc_measurement(100, 0); |
} |
void gyro_init(void) { |
gyro_calibrate(); |
} |
void gyro_setDefaultParameters(void) { |
IMUConfig.driftCompDivider = 2; |
IMUConfig.driftCompLimit = 5; |
IMUConfig.zerothOrderCorrection = 1; |
IMUConfig.imuReversedFlags = IMU_REVERSE_ACC_Z; |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/invenSense.d |
---|
0,0 → 1,0 |
invenSense.o invenSense.d : invenSense.c timer0.h configuration.h |
/branches/dongfang_FC_fixedwing/arduino_atmega328/invenSense.lst |
---|
0,0 → 1,77 |
1 .file "invenSense.c" |
2 __SREG__ = 0x3f |
3 __SP_H__ = 0x3e |
4 __SP_L__ = 0x3d |
5 __CCP__ = 0x34 |
6 __tmp_reg__ = 0 |
7 __zero_reg__ = 1 |
8 .text |
9 .global gyro_calibrate |
11 gyro_calibrate: |
12 /* prologue: function */ |
13 /* frame size = 0 */ |
14 /* stack size = 0 */ |
15 .L__stack_usage = 0 |
16 0000 559B sbis 42-32,5 |
17 0002 00C0 rjmp .L2 |
18 0004 5D99 sbic 43-32,5 |
19 0006 00C0 rjmp .L3 |
20 .L2: |
21 0008 5D9A sbi 43-32,5 |
22 000a 559A sbi 42-32,5 |
23 000c 84E6 ldi r24,lo8(100) |
24 000e 90E0 ldi r25,hi8(100) |
25 0010 0E94 0000 call delay_ms |
26 .L3: |
27 0014 5D98 cbi 43-32,5 |
28 0016 81E0 ldi r24,lo8(1) |
29 0018 90E0 ldi r25,hi8(1) |
30 001a 0E94 0000 call delay_ms |
31 001e 5D9A sbi 43-32,5 |
32 0020 84E6 ldi r24,lo8(100) |
33 0022 90E0 ldi r25,hi8(100) |
34 0024 60E0 ldi r22,lo8(0) |
35 0026 0E94 0000 call delay_ms_with_adc_measurement |
36 /* epilogue start */ |
37 002a 0895 ret |
39 .global gyro_init |
41 gyro_init: |
42 /* prologue: function */ |
43 /* frame size = 0 */ |
44 /* stack size = 0 */ |
45 .L__stack_usage = 0 |
46 002c 0E94 0000 call gyro_calibrate |
47 /* epilogue start */ |
48 0030 0895 ret |
50 .global gyro_setDefaultParameters |
52 gyro_setDefaultParameters: |
53 /* prologue: function */ |
54 /* frame size = 0 */ |
55 /* stack size = 0 */ |
56 .L__stack_usage = 0 |
57 0032 82E0 ldi r24,lo8(2) |
58 0034 8093 0000 sts IMUConfig+10,r24 |
59 0038 85E0 ldi r24,lo8(5) |
60 003a 8093 0000 sts IMUConfig+11,r24 |
61 003e 81E0 ldi r24,lo8(1) |
62 0040 8093 0000 sts IMUConfig+7,r24 |
63 0044 88E0 ldi r24,lo8(8) |
64 0046 8093 0000 sts IMUConfig+2,r24 |
65 /* epilogue start */ |
66 004a 0895 ret |
DEFINED SYMBOLS |
*ABS*:0000000000000000 invenSense.c |
/tmp/ccNP4kuT.s:2 *ABS*:000000000000003f __SREG__ |
/tmp/ccNP4kuT.s:3 *ABS*:000000000000003e __SP_H__ |
/tmp/ccNP4kuT.s:4 *ABS*:000000000000003d __SP_L__ |
/tmp/ccNP4kuT.s:5 *ABS*:0000000000000034 __CCP__ |
/tmp/ccNP4kuT.s:6 *ABS*:0000000000000000 __tmp_reg__ |
/tmp/ccNP4kuT.s:7 *ABS*:0000000000000001 __zero_reg__ |
/tmp/ccNP4kuT.s:11 .text:0000000000000000 gyro_calibrate |
/tmp/ccNP4kuT.s:41 .text:000000000000002c gyro_init |
/tmp/ccNP4kuT.s:52 .text:0000000000000032 gyro_setDefaultParameters |
UNDEFINED SYMBOLS |
delay_ms |
delay_ms_with_adc_measurement |
IMUConfig |
/branches/dongfang_FC_fixedwing/arduino_atmega328/isqrt.S |
---|
0,0 → 1,203 |
;-----------------------------------------------------------------------------; |
; Fast integer squareroot routines for avr-gcc project (C)ChaN, 2008 |
; http://elm-chan.org/docs/avrlib/sqrt32.S |
;-----------------------------------------------------------------------------; |
; uint16_t isqrt32 (uint32_t n); |
; uint8_t isqrt16 (uint16_t n); |
; uint16_t ihypot (int16_t x, int16_t y); |
;-----------------------------------------------------------------------------: |
; 32bit integer squareroot |
;-----------------------------------------------------------------------------; |
; uint16_t isqrt32 ( |
; uint32_t n |
; ); |
; |
; Return Value: |
; Squareroot of n. |
; |
; Size = 53 words |
; Clock = 532..548 cycles |
; Stack = 0 byte |
.global isqrt32 |
.func isqrt32 |
isqrt32: |
clr r0 |
clr r18 |
clr r19 |
clr r20 |
ldi r21, 1 |
clr r27 |
clr r30 |
clr r31 |
ldi r26, 16 |
1: lsl r22 |
rol r23 |
rol r24 |
rol r25 |
rol r0 |
rol r18 |
rol r19 |
rol r20 |
lsl r22 |
rol r23 |
rol r24 |
rol r25 |
rol r0 |
rol r18 |
rol r19 |
rol r20 |
brpl 2f |
add r0, r21 |
adc r18, r27 |
adc r19, r30 |
adc r20, r31 |
rjmp 3f |
2: sub r0, r21 |
sbc r18, r27 |
sbc r19, r30 |
sbc r20, r31 |
3: lsl r21 |
rol r27 |
rol r30 |
andi r21, 0b11111000 |
ori r21, 0b00000101 |
sbrc r20, 7 |
subi r21, 2 |
dec r26 |
brne 1b |
lsr r30 |
ror r27 |
ror r21 |
lsr r30 |
ror r27 |
ror r21 |
mov r24, r21 |
mov r25, r27 |
ret |
.endfunc |
;-----------------------------------------------------------------------------: |
; 16bit integer squareroot |
;-----------------------------------------------------------------------------; |
; uint8_t isqrt16 ( |
; uint16_t n |
; ); |
; |
; Return Value: |
; Squareroot of n. |
; |
; Size = 33 words |
; Clock = 181..189 cycles |
; Stack = 0 byte |
.global isqrt16 |
.func isqrt16 |
isqrt16: |
clr r18 |
clr r19 |
ldi r20, 1 |
clr r21 |
ldi r22, 8 |
1: lsl r24 |
rol r25 |
rol r18 |
rol r19 |
lsl r24 |
rol r25 |
rol r18 |
rol r19 |
brpl 2f |
add r18, r20 |
adc r19, r21 |
rjmp 3f |
2: sub r18, r20 |
sbc r19, r21 |
3: lsl r20 |
rol r21 |
andi r20, 0b11111000 |
ori r20, 0b00000101 |
sbrc r19, 7 |
subi r20, 2 |
dec r22 |
brne 1b |
lsr r21 |
ror r20 |
lsr r21 |
ror r20 |
mov r24, r20 |
ret |
.endfunc |
;-----------------------------------------------------------------------------: |
; 16bit integer hypot (megaAVR is required) |
;-----------------------------------------------------------------------------; |
; uint16_t ihypot ( |
; int16_t x, |
; int16_t y |
; ); |
; |
; Return Value: |
; Squareroot of (x*x + y*y) |
; |
; Size = 42 words |
; Clock = 581..597 cycles |
; Stack = 0 byte |
.global ihypot |
.func ihypot |
ihypot: |
clr r26 |
sbrs r25, 7 |
rjmp 1f |
com r24 |
com r25 |
adc r24, r26 |
adc r25, r26 |
1: sbrs r23, 7 |
rjmp 2f |
com r22 |
com r23 |
adc r22, r26 |
adc r23, r26 |
2: mul r22, r22 |
movw r18, r0 |
mul r23, r23 |
movw r20, r0 |
mul r22, r23 |
add r19, r0 |
adc r20, r1 |
adc r21, r26 |
add r19, r0 |
adc r20, r1 |
adc r21, r26 |
mul r24, r24 |
movw r30, r0 |
mul r25, r25 |
add r18, r30 |
adc r19, r31 |
adc r20, r0 |
adc r21, r1 |
mul r24, r25 |
add r19, r0 |
adc r20, r1 |
adc r21, r26 |
add r19, r0 |
adc r20, r1 |
adc r21, r26 |
movw r24, r20 |
movw r22, r18 |
clr r1 |
rjmp isqrt32 |
.endfunc |
/branches/dongfang_FC_fixedwing/arduino_atmega328/isqrt.h |
---|
0,0 → 1,11 |
#ifndef _ISQRT_H |
#define _ISQRT_H |
#include <inttypes.h> |
// coded in assembler file |
extern uint16_t isqrt32(uint32_t n); |
extern uint8_t isqrt16(uint16_t n); |
extern uint16_t ihypot(int16_t x, int16_t y); |
#endif // _ISQRT_H |
/branches/dongfang_FC_fixedwing/arduino_atmega328/main.c |
---|
0,0 → 1,170 |
#include <avr/boot.h> |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <util/delay.h> |
#include "timer0.h" |
#include "timer2.h" |
#include "uart0.h" |
#include "output.h" |
#include "attitude.h" |
#include "commands.h" |
#include "flight.h" |
#include "rc.h" |
#include "analog.h" |
#include "configuration.h" |
#include "controlMixer.h" |
#include "eeprom.h" |
#include "printf_P.h" |
int16_t main(void) { |
uint16_t timer; |
// disable interrupts global |
cli(); |
// disable watchdog |
MCUSR &= ~(1 << WDRF); |
WDTCSR |= (1 << WDCE) | (1 << WDE); |
WDTCSR = 0; |
// This is strange: It should NOT be necessarty to do. But the call of the same, |
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,.... |
channelMap_default(); |
// initalize modules |
output_init(); |
timer0_init(); |
timer2_init(); |
usart0_init(); |
RC_Init(); |
analog_init(); |
// Parameter Set handling |
IMUConfig_readOrDefault(); |
channelMap_readOrDefault(); |
paramSet_readOrDefault(); |
// enable interrupts global |
sei(); |
printf("\n\r==================================="); |
printf("\n\rFlightControl"); |
printf("\n\rHardware: Custom"); |
printf("\n\r CPU: Atmega328"); |
printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
printf("\n\r==================================="); |
// Wait for a short time (otherwise the RC channel check won't work below) |
// timer = SetDelay(500); |
// while(!CheckDelay(timer)); |
// Instead, while away the time by flashing the 2 outputs: |
// First J16, then J17. Makes it easier to see which is which. |
timer = setDelay(200); |
outputSet(0,1); |
GRN_OFF; |
RED_ON; |
while (!checkDelay(timer)) |
; |
timer = setDelay(200); |
outputSet(0,0); |
outputSet(1,1); |
RED_OFF; |
GRN_ON; |
while (!checkDelay(timer)) |
; |
timer = setDelay(200); |
while (!checkDelay(timer)) |
; |
outputSet(1,0); |
GRN_OFF; |
printf("\n\r==================================="); |
#ifdef USE_NAVICTRL |
printf("\n\rSupport for NaviCtrl"); |
#endif |
#ifdef USE_DIRECT_GPS |
printf("\n\rDirect (no NaviCtrl) navigation"); |
#endif |
controlMixer_setNeutral(); |
// Cal. attitude sensors and reset integrals. |
attitude_setNeutral(); |
// Init flight parameters |
// flight_setNeutral(); |
beep(2000); |
printf("\n\n\r"); |
while (1) { |
if (runFlightControl) { // control interval |
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
if (sensorDataReady != ALL_DATA_READY) { |
// Analog data should have been ready but is not!! |
debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER; |
} else { |
debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
J4HIGH; |
// This is probably the correct order: |
// The attitude computation should not depend on anything from control (except maybe the estimation of control activity level) |
// The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc. |
// Flight control uses results from both. |
calculateFlightAttitude(); |
controlMixer_periodicTask(); |
commands_handleCommands(); |
flight_control(); |
J4LOW; |
// Allow Serial Data Transmit if motors must not updated or motors are not running |
if (!runFlightControl || !isFlying) { |
usart0_transmitTxData(); |
} |
usart0_processRxData(); |
if (checkDelay(timer)) { |
if (UBat <= UBAT_AT_5V) { |
// Do nothing. The voltage on the input side of the regulator is <5V; |
// we must be running off USB power. Keep it quiet. |
} else if (UBat < staticParams.batteryVoltageWarning) { |
beepBatteryAlarm(); |
} |
#ifdef USE_NAVICTRL |
SPI_StartTransmitPacket(); |
SendSPI = 4; |
#endif |
timer = setDelay(20); // every 20 ms |
} |
output_update(); |
} |
#ifdef USE_NAVICTRL |
if(!SendSPI) { |
// SendSPI is decremented in timer0.c with a rate of 9.765 kHz. |
// within the SPI_TransmitByte() routine the value is set to 4. |
// I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz, |
// and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms. |
SPI_TransmitByte(); |
} |
#endif |
calculateFeaturedServoValues(); |
if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |
} else { |
debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
} |
} |
} |
return (1); |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/makefile |
---|
0,0 → 1,387 |
#-------------------------------------------------------------------- |
# MCU name |
MCU = atmega328 |
F_CPU = 16000000 |
QUARZ = 16MHZ |
#------------------------------------------------------------------- |
VERSION_MAJOR = 0 |
VERSION_MINOR = 0 |
VERSION_PATCH = 0 |
VERSION_SERIAL_MAJOR = 10 # Serial Protocol Major Version |
VERSION_SERIAL_MINOR = 1 # Serial Protocol Minor Version |
NC_SPI_COMPATIBLE = 6 # SPI Protocol Version |
# Set up for cross axis gyros. Factors increased by sqrt(2). |
#GYRO=ADXRS610_FC2.0 |
#GYRO_HW_NAME=ADXR |
#GYRO_HW_FACTOR="(1.2288f * 1.4142)" |
#GYRO_CORRECTION=1.0f |
GYRO=invenSense |
GYRO_HW_NAME=Isense |
GYRO_HW_FACTOR=0.6827f |
GYRO_CORRECTION=1.0f |
#------------------------------------------------------------------- |
# get SVN revision |
REV=0001 |
#$(shell sh -c "cat .svn/entries | sed -n '4p'") |
ifeq ($(MCU), atmega328) |
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m |
HEX_NAME = MEGA328_$(GYRO) |
endif |
# Output format. (can be srec, ihex, binary) |
FORMAT = ihex |
# Target file name (without extension). |
TARGET = Flight-Ctrl_Fixedwing_$(HEX_NAME) |
# Optimization level, can be [0, 1, 2, 3, s]. 0 turns off optimization. |
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.) |
OPT = 2 |
#OPT = s |
########################################################################################################## |
# List C source files here. (C dependencies are automatically generated.) |
SRC = main.c uart0.c timer0.c timer2.c analog.c output.c controlMixer.c |
SRC += rc.c failsafeControl.c attitude.c flight.c printf_P.c commands.c |
SRC += eeprom.c configuration.c externalControl.c twimaster.c |
########################################################################################################## |
# List Assembler source files here. |
# Make them always end in a capital .S. Files ending in a lowercase .s |
# will not be considered source files but generated files (assembler |
# output from the compiler), and will be deleted upon "make clean"! |
# Even though the DOS/Win* filesystem matches both .s and .S the same, |
# it will preserve the spelling of the filenames, and gcc itself does |
# care about how the name is spelled on its command-line. |
ASRC = isqrt.S |
# List any extra directories to look for include files here. |
# Each directory must be seperated by a space. |
EXTRAINCDIRS = |
# Optional compiler flags. |
# -g: generate debugging information (for GDB, or for COFF conversion) |
# -O*: optimization level |
# -f...: tuning, see gcc manual and avr-libc documentation |
# -Wall...: warning level |
# -Wa,...: tell GCC to pass this to the assembler. |
# -ahlms: create assembler listing |
CFLAGS = -O$(OPT) \ |
-funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums \ |
-Wall -Wstrict-prototypes \ |
-DGYRO=$(GYRO) \ |
-Wa,-adhlns=$(<:.c=.lst) \ |
$(patsubst %,-I%,$(EXTRAINCDIRS)) |
# Set a "language standard" compiler flag. |
# Unremark just one line below to set the language standard to use. |
# gnu99 = C99 + GNU extensions. See GCC manual for more information. |
#CFLAGS += -std=c89 |
#CFLAGS += -std=gnu89 |
#CFLAGS += -std=c99 |
CFLAGS += -std=gnu99 |
CFLAGS += -DF_CPU=$(F_CPU) -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_PATCH=$(VERSION_PATCH) -DVERSION_SERIAL_MAJOR=$(VERSION_SERIAL_MAJOR) -DVERSION_SERIAL_MINOR=$(VERSION_SERIAL_MINOR) -DNC_SPI_COMPATIBLE=$(NC_SPI_COMPATIBLE) -DGYRO_HW_NAME=${GYRO_HW_NAME} -DGYRO_HW_FACTOR=${GYRO_HW_FACTOR} -DGYRO_CORRECTION=${GYRO_CORRECTION} |
# Optional assembler flags. |
# -Wa,...: tell GCC to pass this to the assembler. |
# -ahlms: create listing |
# -gstabs: have the assembler create line number information; note that |
# for use in COFF files, additional information about filenames |
# and function names needs to be present in the assembler source |
# files -- see avr-libc docs [FIXME: not yet described there] |
ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs |
# Optional linker flags. |
# -Wl,...: tell GCC to pass this to linker. |
# -Map: create map file |
# --cref: add cross reference to map file |
LDFLAGS = -Wl,-Map=$(TARGET).map,--cref |
# Additional libraries |
# Minimalistic printf version |
#LDFLAGS += -Wl,-u,vfprintf -lprintf_min |
# Floating point printf version (requires -lm below) |
#LDFLAGS += -Wl,-u,vfprintf -lprintf_flt |
# -lm = math library |
LDFLAGS += -lm |
##LDFLAGS += -T./linkerfile/avr5.x |
# Programming support using avrdude. Settings and variables. |
# Programming hardware: alf avr910 avrisp bascom bsd |
# dt006 pavr picoweb pony-stk200 sp12 stk200 stk500 |
# |
# Type: avrdude -c ? |
# to get a full listing. |
# |
#AVRDUDE_PROGRAMMER = dt006 |
#AVRDUDE_PROGRAMMER = stk200 |
#AVRDUDE_PROGRAMMER = ponyser |
#AVRDUDE_PROGRAMMER = avrispv2 |
AVRDUDE_PROGRAMMER = usbtiny |
#falls Ponyser ausgewaehlt wird, muss sich unsere avrdude-Configdatei im Bin-Verzeichnis des Compilers befinden |
#AVRDUDE_PORT = com1 # programmer connected to serial device |
#AVRDUDE_PORT = lpt1 # programmer connected to parallel port |
AVRDUDE_PORT = usb # programmer connected to USB |
#AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex |
AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex $(FUSE_SETTINGS) |
#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep |
#avrdude -c avrispv2 -P usb -p m32 -U flash:w:blink.hex |
AVRDUDE_FLAGS = -p $(MCU) -c $(AVRDUDE_PROGRAMMER) -B 1 -F |
# Uncomment the following if you want avrdude's erase cycle counter. |
# Note that this counter needs to be initialized first using -Yn, |
# see avrdude manual. |
#AVRDUDE_ERASE += -y |
# Uncomment the following if you do /not/ wish a verification to be |
# performed after programming the device. |
AVRDUDE_FLAGS += -V |
# Increase verbosity level. Please use this when submitting bug |
# reports about avrdude. See <http://savannah.nongnu.org/projects/avrdude> |
# to submit bug reports. |
#AVRDUDE_FLAGS += -v -v |
# --------------------------------------------------------------------------- |
# Define directories, if needed. |
DIRAVR = c:/winavr |
DIRAVRBIN = $(DIRAVR)/bin |
DIRAVRUTILS = $(DIRAVR)/utils/bin |
DIRINC = . |
DIRLIB = $(DIRAVR)/avr/lib |
# Define programs and commands. |
SHELL = sh |
CC = avr-gcc |
OBJCOPY = avr-objcopy |
OBJDUMP = avr-objdump |
SIZE = avr-size |
# Programming support using avrdude. |
AVRDUDE = avrdude |
REMOVE = rm -f |
COPY = cp |
HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex |
ELFSIZE = $(SIZE) -A $(TARGET).elf |
# Define Messages |
# English |
MSG_ERRORS_NONE = Errors: none |
MSG_BEGIN = -------- begin -------- |
MSG_END = -------- end -------- |
MSG_SIZE_BEFORE = Size before: |
MSG_SIZE_AFTER = Size after: |
MSG_COFF = Converting to AVR COFF: |
MSG_EXTENDED_COFF = Converting to AVR Extended COFF: |
MSG_FLASH = Creating load file for Flash: |
MSG_EEPROM = Creating load file for EEPROM: |
MSG_EXTENDED_LISTING = Creating Extended Listing: |
MSG_SYMBOL_TABLE = Creating Symbol Table: |
MSG_LINKING = Linking: |
MSG_COMPILING = Compiling: |
MSG_ASSEMBLING = Assembling: |
MSG_CLEANING = Cleaning project: |
# Define all object files. |
OBJ = $(SRC:.c=.o) $(ASRC:.S=.o) |
# Define all listing files. |
LST = $(ASRC:.S=.lst) $(SRC:.c=.lst) |
# Combine all necessary flags and optional flags. |
# Add target processor to flags. |
#ALL_CFLAGS = -mmcu=$(MCU) -DF_CPU=$(F_CPU) -I. $(CFLAGS) |
ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) |
ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS) |
# Default target. |
all: begin gccversion sizebefore $(TARGET).elf $(TARGET).hex $(TARGET).eep \ |
$(TARGET).lss $(TARGET).sym sizeafter finished end |
# Eye candy. |
# AVR Studio 3.x does not check make's exit code but relies on |
# the following magic strings to be generated by the compile job. |
begin: |
@echo |
@echo $(MSG_BEGIN) |
finished: |
@echo $(MSG_ERRORS_NONE) |
end: |
@echo $(MSG_END) |
@echo |
# Display size of file. |
sizebefore: |
@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi |
sizeafter: |
@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi |
# Display compiler version information. |
gccversion : |
@$(CC) --version |
# Convert ELF to COFF for use in debugging / simulating in |
# AVR Studio or VMLAB. |
COFFCONVERT=$(OBJCOPY) --debugging \ |
--change-section-address .data-0x800000 \ |
--change-section-address .bss-0x800000 \ |
--change-section-address .noinit-0x800000 \ |
--change-section-address .eeprom-0x810000 |
coff: $(TARGET).elf |
@echo |
@echo $(MSG_COFF) $(TARGET).cof |
$(COFFCONVERT) -O coff-avr $< $(TARGET).cof |
extcoff: $(TARGET).elf |
@echo |
@echo $(MSG_EXTENDED_COFF) $(TARGET).cof |
$(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof |
# Program the device. |
program: $(TARGET).hex $(TARGET).eep |
$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM) |
# Create final output files (.hex, .eep) from ELF output file. |
%.hex: %.elf |
@echo |
@echo $(MSG_FLASH) $@ |
$(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@ |
%.eep: %.elf |
@echo |
@echo $(MSG_EEPROM) $@ |
-$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \ |
--change-section-lma .eeprom=0 -O $(FORMAT) $< $@ |
# Create extended listing file from ELF output file. |
%.lss: %.elf |
@echo |
@echo $(MSG_EXTENDED_LISTING) $@ |
$(OBJDUMP) -h -S $< > $@ |
# Create a symbol table from ELF output file. |
%.sym: %.elf |
@echo |
@echo $(MSG_SYMBOL_TABLE) $@ |
avr-nm -n $< > $@ |
# Link: create ELF output file from object files. |
.SECONDARY : $(TARGET).elf |
.PRECIOUS : $(OBJ) |
%.elf: $(OBJ) |
@echo |
@echo $(MSG_LINKING) $@ |
$(CC) $(ALL_CFLAGS) $(OBJ) --output $@ $(LDFLAGS) |
# Compile: create object files from C source files. |
%.o : %.c |
@echo |
@echo $(MSG_COMPILING) $< |
$(CC) -c $(ALL_CFLAGS) $< -o $@ |
# Compile: create assembler files from C source files. |
%.s : %.c |
$(CC) -S $(ALL_CFLAGS) $< -o $@ |
# Assemble: create object files from assembler source files. |
%.o : %.S |
@echo |
@echo $(MSG_ASSEMBLING) $< |
$(CC) -c $(ALL_ASFLAGS) $< -o $@ |
# Target: clean project. |
clean: begin clean_list finished end |
clean_list : |
@echo |
@echo $(MSG_CLEANING) |
$(REMOVE) $(TARGET).hex |
$(REMOVE) $(TARGET).eep |
$(REMOVE) $(TARGET).obj |
$(REMOVE) $(TARGET).cof |
$(REMOVE) $(TARGET).elf |
$(REMOVE) $(TARGET).map |
$(REMOVE) $(TARGET).obj |
$(REMOVE) $(TARGET).a90 |
$(REMOVE) $(TARGET).sym |
$(REMOVE) $(TARGET).lnk |
$(REMOVE) $(TARGET).lss |
$(REMOVE) $(OBJ) |
$(REMOVE) $(LST) |
$(REMOVE) $(SRC:.c=.s) |
$(REMOVE) $(SRC:.c=.d) |
# Automatically generate C source code dependencies. |
# (Code originally taken from the GNU make user manual and modified |
# (See README.txt Credits).) |
# |
# Note that this will work with sh (bash) and sed that is shipped with WinAVR |
# (see the SHELL variable defined above). |
# This may not work with other shells or other seds. |
# |
%.d: %.c |
set -e; $(CC) -MM $(ALL_CFLAGS) $< \ |
| sed 's,\(.*\)\.o[ :]*,\1.o \1.d : ,g' > $@; \ |
[ -s $@ ] || rm -f $@ |
# Remove the '-' if you want to see the dependency files generated. |
-include $(SRC:.c=.d) |
# Listing of phony targets. |
.PHONY : all begin finish end sizebefore sizeafter gccversion coff extcoff \ |
clean clean_list program |
/branches/dongfang_FC_fixedwing/arduino_atmega328/old_macros.h |
---|
0,0 → 1,47 |
/* |
For backwards compatibility only. |
Ingo Busker ingo@mikrocontroller.com |
*/ |
#ifndef cbi |
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) |
#endif |
#ifndef sbi |
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) |
#endif |
#ifndef inb |
#define inb(sfr) _SFR_BYTE(sfr) |
#endif |
#ifndef outb |
#define outb(sfr, val) (_SFR_BYTE(sfr) = (val)) |
#endif |
#ifndef inw |
#define inw(sfr) _SFR_WORD(sfr) |
#endif |
#ifndef outw |
#define outw(sfr, val) (_SFR_WORD(sfr) = (val)) |
#endif |
#ifndef outp |
#define outp(val, sfr) outb(sfr, val) |
#endif |
#ifndef inp |
#define inp(sfr) inb(sfr) |
#endif |
#ifndef BV |
#define BV(bit) _BV(bit) |
#endif |
#ifndef PRG_RDB |
#define PRG_RDB pgm_read_byte |
#endif |
/branches/dongfang_FC_fixedwing/arduino_atmega328/output.c |
---|
0,0 → 1,125 |
#include <inttypes.h> |
#include "output.h" |
#include "eeprom.h" |
#include "timer0.h" |
uint8_t flashCnt[2], flashMask[2]; |
DebugOut_t debugOut; |
void output_init(void) { |
// set PC2 & PC3 as output (control of J16 & J17) |
DDRB |= (1 << DDB1) | (1 << DDB2); |
outputSet(0,0); |
outputSet(1,0); |
flashCnt[0] = flashCnt[1] = 0; |
flashMask[0] = flashMask[1] = 128; |
} |
void outputSet(uint8_t num, uint8_t state) { |
if (staticParams.outputFlags & (OUTPUTFLAGS_INVERT_0 << num)) { |
if (state) OUTPUT_LOW(num) else OUTPUT_HIGH(num); |
} else { |
if (state) OUTPUT_HIGH(num) else OUTPUT_LOW(num); |
} |
if (staticParams.outputFlags & OUTPUTFLAGS_USE_ONBOARD_LEDS) { |
if (num) { |
if (state) GRN_ON else GRN_OFF; |
} else { |
if (state) RED_ON else RED_OFF; |
} |
} |
} |
void flashingLight(uint8_t port, uint8_t timing, uint8_t bitmask, uint8_t manual) { |
if (timing > 250 && manual > 230) { |
// "timing" is set to "manual (a variable)" and the value is very high --> Set to the value in bitmask bit 7. |
outputSet(port, 1); |
} else if (timing > 250 && manual < 10) { |
// "timing" is set to "manual" (a variable) and the value is very low --> Set to the negated value in bitmask bit 7. |
outputSet(port, 0); |
} else if (!flashCnt[port]--) { |
// rotating mask over bitmask... |
flashCnt[port] = timing - 1; |
if (flashMask[port] == 1) |
flashMask[port] = 128; |
else |
flashMask[port] >>= 1; |
outputSet(port, flashMask[port] & bitmask); |
} |
} |
void output_update(void) { |
static int8_t delay = 0; |
if (!delay--) { // 10 ms intervals |
delay = 4; |
} |
if (staticParams.outputFlags & OUTPUTFLAGS_TEST_ON) { |
outputSet(0, 1); |
outputSet(1, 1); |
} else if (staticParams.outputFlags & OUTPUTFLAGS_TEST_OFF) { |
outputSet(0, 0); |
outputSet(1, 0); |
} else { |
if (staticParams.outputFlags & OUTPUTFLAGS_FLASH_0_AT_BEEP && beepModulation != BEEP_MODULATION_NONE) { |
flashingLight(0, 25, 0x55, 25); |
} else if (staticParams.outputDebugMask) { |
outputSet(0, debugOut.digital[0] & staticParams.outputDebugMask); |
} else if (!delay) { |
flashingLight(0, staticParams.outputFlash[0].timing, staticParams.outputFlash[0].bitmask, dynamicParams.output0Timing); |
} |
if (staticParams.outputFlags & OUTPUTFLAGS_FLASH_1_AT_BEEP && beepModulation != BEEP_MODULATION_NONE) { |
flashingLight(1, 25, 0x55, 25); |
} else if (staticParams.outputDebugMask) { |
outputSet(1, debugOut.digital[1] & staticParams.outputDebugMask); |
} else if (!delay) { |
flashingLight(1, staticParams.outputFlash[1].timing, staticParams.outputFlash[1].bitmask, dynamicParams.output1Timing); |
} |
} |
} |
void beep(uint16_t millis) { |
beepTime = millis; |
} |
/* |
* Make [numbeeps] beeps. |
*/ |
void beepNumber(uint8_t numbeeps) { |
while(numbeeps--) { |
if(isFlying) return; //auf keinen Fall bei laufenden Motoren! |
beep(100); // 0.1 second |
delay_ms(250); // blocks 250 ms as pause to next beep, |
// this will block the flight control loop, |
// therefore do not use this function if motors are running |
} |
} |
/* |
* Beep the R/C alarm signal |
*/ |
void beepRCAlarm(void) { |
if(beepModulation == BEEP_MODULATION_NONE) { // If not already beeping an alarm signal (?) |
beepTime = 15000; // 1.5 seconds |
beepModulation = BEEP_MODULATION_RCALARM; |
} |
} |
/* |
* Beep the battery low alarm signal |
*/ |
void beepBatteryAlarm(void) { |
beepModulation = BEEP_MODULATION_BATTERYALARM; |
if(!beepTime) { |
beepTime = 6000; // 0.6 seconds |
} |
} |
/* |
* Beep the EEPROM checksum alarm |
*/ |
void beepEEPROMAlarm(void) { |
beepModulation = BEEP_MODULATION_EEPROMALARM; |
if(!beepTime) { |
beepTime = 6000; // 0.6 seconds |
} |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/output.h |
---|
0,0 → 1,86 |
#ifndef _OUTPUT_H |
#define _OUTPUT_H |
#include <avr/io.h> |
#define J3HIGH PORTD |= (1<<PORTD5) |
#define J3LOW PORTD &= ~(1<<PORTD5) |
#define J3TOGGLE PORTD ^= (1<<PORTD5) |
#define J4HIGH PORTD |= (1<<PORTD4) |
#define J4LOW PORTD &= ~(1<<PORTD4) |
#define J4TOGGLE PORTD ^= (1<<PORTD4) |
#define J5HIGH PORTD |= (1<<PORTD3) |
#define J5LOW PORTD &= ~(1<<PORTD3) |
#define J5TOGGLE PORTD ^= (1<<PORTD3) |
// invert means: An "1" bit in digital debug data make a LOW on the output. |
#define DIGITAL_DEBUG_INVERT 0 |
#define OUTPUT_HIGH(num) {PORTB |= (2 << (num));} |
#define OUTPUT_LOW(num) {PORTB &= ~(2 << (num));} |
#define OUTPUT_TOGGLE(num) ( {PORTB ^= (2 << (num));} |
/* |
* Some digital debugs. A digital debug is 2 signals on the 2 LED outputs, |
* turned on and off depending on some condtions given in the code. |
* Only one can be selected, by defining DIGITAL_DEBUG_MASK to the value |
* of the debug. |
* In the code one can do like: |
* if (whatever_condition) { |
* DebugOut.Digital[0] |= DEBUG_MYOWNDEBUGGER; |
* } else { |
* DebugOut.Digital[0] &= ~DEBUG_MYOWNDEBUGGER; |
* } |
* ... |
* if (whatever_other_condition) { |
* DebugOut.Digital[1] |= DEBUG_MYOWNDEBUGGER; |
* } else { |
* DebugOut.Digital[1] &= ~DEBUG_MYOWNDEBUGGER; |
* } |
* |
* Digital debugs may be added as desired, and removed when the mystery |
* at hand is resolved. |
*/ |
#define DEBUG_MAINLOOP_TIMER 1 |
#define DEBUG_HEIGHT_DIFF 2 |
#define DEBUG_FLIGHTCLIP 4 |
#define DEBUG_ACC0THORDER 8 |
#define DEBUG_COMPASS 16 |
#define DEBUG_PRESSURERANGE 32 |
#define DEBUG_CLIP 64 |
#define DEBUG_SENSORLIMIT 128 |
#define OUTPUTFLAGS_INVERT_0 1 // Inverted: 1 means low output on atmega. Does not affect on-board LED (if used with the OUTPUTOPTIONS_USE_ONBOARD_LEDS option) |
#define OUTPUTFLAGS_INVERT_1 2 // Inverted: 1 means low output on atmega. Does not affect on-board LED (if used with the OUTPUTOPTIONS_USE_ONBOARD_LEDS option) |
#define OUTPUTFLAGS_FLASH_0_AT_BEEP 4 // Flash LED when beeper beeps |
#define OUTPUTFLAGS_FLASH_1_AT_BEEP 8 // Flash LED when beeper beeps |
#define OUTPUTFLAGS_USE_ONBOARD_LEDS 16 // Control on-board LEDs in addition to outputs |
#define OUTPUTFLAGS_TEST_OFF 32 // For testing: Turn off both outputs |
#define OUTPUTFLAGS_TEST_ON 64 // For testing: Turn on both outputs |
// For practical reasons put here instead of in uart0.h |
typedef struct { |
uint8_t digital[2]; |
uint16_t analog[32]; // debug values |
}__attribute__((packed)) DebugOut_t; |
extern DebugOut_t debugOut; |
/* |
* Set to 0 for using outputs as the usual flashing lights. |
* Set to one of the DEBUG_... defines h for using the outputs as debug lights. |
*/ |
#define DIGITAL_DEBUG_MASK 0 |
void output_init(void); |
void outputSet(uint8_t num, uint8_t state); |
void output_update(void); |
void beep(uint16_t millis); |
void beepNumber(uint8_t numbeeps); |
void beepRCAlarm(void); |
void beepBatteryAlarm(void); |
#endif //_output_H |
/branches/dongfang_FC_fixedwing/arduino_atmega328/printf_P.c |
---|
0,0 → 1,482 |
// Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist nicht von der Lizenz für den MikroKopter-Teil unterstellt |
/* |
Copyright (C) 1993 Free Software Foundation |
This file is part of the GNU IO Library. This library is free |
software; you can redistribute it and/or modify it under the |
terms of the GNU General Public License as published by the |
Free Software Foundation; either version 2, or (at your option) |
any later version. |
This library is distributed in the hope that it will be useful, |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
GNU General Public License for more details. |
You should have received a copy of the GNU General Public License |
along with this library; see the file COPYING. If not, write to the Free |
Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. |
As a special exception, if you link this library with files |
compiled with a GNU compiler to produce an executable, this does not cause |
the resulting executable to be covered by the GNU General Public License. |
This exception does not however invalidate any other reasons why |
the executable file might be covered by the GNU General Public License. */ |
/* |
* Copyright (c) 1990 Regents of the University of California. |
* All rights reserved. |
* |
* Redistribution and use in source and binary forms, with or without |
* modification, are permitted provided that the following conditions |
* are met: |
* 1. Redistributions of source code must retain the above copyright |
* notice, this list of conditions and the following disclaimer. |
* 2. Redistributions in binary form must reproduce the above copyright |
* notice, this list of conditions and the following disclaimer in the |
* documentation and/or other materials provided with the distribution. |
* 3. [rescinded 22 July 1999] |
* 4. Neither the name of the University nor the names of its contributors |
* may be used to endorse or promote products derived from this software |
* without specific prior written permission. |
* |
* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND |
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
* ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE |
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS |
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY |
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF |
* SUCH DAMAGE. |
*/ |
/****************************************************************************** |
This file is a patched version of printf called _printf_P |
It is made to work with avr-gcc for Atmel AVR MCUs. |
There are some differences from standard printf: |
1. There is no floating point support (with fp the code is about 8K!) |
2. Return type is void |
3. Format string must be in program memory (by using macro printf this is |
done automaticaly) |
4. %n is not implemented (just remove the comment around it if you need it) |
5. If LIGHTPRINTF is defined, the code is about 550 bytes smaller and the |
folowing specifiers are disabled : |
space # * . - + p s o O |
6. A function void uart_sendchar(char c) is used for output. The UART must |
be initialized before using printf. |
Alexander Popov |
sasho@vip.orbitel.bg |
******************************************************************************/ |
/* |
* Actual printf innards. |
* |
* This code is large and complicated... |
*/ |
#include <string.h> |
#ifdef __STDC__ |
#include <stdarg.h> |
#else |
#include <varargs.h> |
#endif |
#include "old_macros.h" |
#include "printf_P.h" |
#include "uart0.h" |
//#define LIGHTPRINTF |
char PrintZiel; |
char Putchar(char zeichen) |
{ |
if(PrintZiel == OUT_LCD) { displayBuff[dispPtr++] = zeichen; return(1);} |
else return(uart_putchar(zeichen)); |
} |
void PRINT(const char * ptr, unsigned int len) |
{ |
for(;len;len--) Putchar(*ptr++); |
} |
void PRINTP(const char * ptr, unsigned int len) |
{ |
for(;len;len--) Putchar(pgm_read_byte(ptr++)); |
} |
void PAD_SP(signed char howmany) |
{ |
for(;howmany>0;howmany--) Putchar(' '); |
} |
void PAD_0(signed char howmany) |
{ |
for(;howmany>0;howmany--) Putchar('0'); |
} |
#define BUF 40 |
/* |
* Macros for converting digits to letters and vice versa |
*/ |
#define to_digit(c) ((c) - '0') |
#define is_digit(c) ((c)<='9' && (c)>='0') |
#define to_char(n) ((n) + '0') |
/* |
* Flags used during conversion. |
*/ |
#define LONGINT 0x01 /* long integer */ |
#define LONGDBL 0x02 /* long double; unimplemented */ |
#define SHORTINT 0x04 /* short integer */ |
#define ALT 0x08 /* alternate form */ |
#define LADJUST 0x10 /* left adjustment */ |
#define ZEROPAD 0x20 /* zero (as opposed to blank) pad */ |
#define HEXPREFIX 0x40 /* add 0x or 0X prefix */ |
void _printf_P (char ziel,char const *fmt0, ...) /* Works with string from FLASH */ |
{ |
va_list ap; |
register const char *fmt; /* format string */ |
register char ch; /* character from fmt */ |
register int n; /* handy integer (short term usage) */ |
register char *cp; /* handy char pointer (short term usage) */ |
const char *fmark; /* for remembering a place in fmt */ |
register unsigned char flags; /* flags as above */ |
signed char width; /* width from format (%8d), or 0 */ |
signed char prec; /* precision from format (%.3d), or -1 */ |
char sign; /* sign prefix (' ', '+', '-', or \0) */ |
unsigned long _ulong=0; /* integer arguments %[diouxX] */ |
#define OCT 8 |
#define DEC 10 |
#define HEX 16 |
unsigned char base; /* base for [diouxX] conversion */ |
signed char dprec; /* a copy of prec if [diouxX], 0 otherwise */ |
signed char dpad; /* extra 0 padding needed for integers */ |
signed char fieldsz; /* field size expanded by sign, dpad etc */ |
/* The initialization of 'size' is to suppress a warning that |
'size' might be used unitialized. It seems gcc can't |
quite grok this spaghetti code ... */ |
signed char size = 0; /* size of converted field or string */ |
char buf[BUF]; /* space for %c, %[diouxX], %[eEfgG] */ |
char ox[2]; /* space for 0x hex-prefix */ |
PrintZiel = ziel; // bestimmt, LCD oder UART |
va_start(ap, fmt0); |
fmt = fmt0; |
/* |
* Scan the format for conversions (`%' character). |
*/ |
for (;;) { |
for (fmark = fmt; (ch = pgm_read_byte(fmt)) != '\0' && ch != '%'; fmt++) |
/* void */; |
if ((n = fmt - fmark) != 0) { |
PRINTP(fmark, n); |
} |
if (ch == '\0') |
goto done; |
fmt++; /* skip over '%' */ |
flags = 0; |
dprec = 0; |
width = 0; |
prec = -1; |
sign = '\0'; |
rflag: ch = PRG_RDB(fmt++); |
reswitch: |
#ifdef LIGHTPRINTF |
if (ch=='o' || ch=='u' || (ch|0x20)=='x') { |
#else |
if (ch=='u' || (ch|0x20)=='x') { |
#endif |
if (flags&LONGINT) { |
_ulong=va_arg(ap, unsigned long); |
} else { |
register unsigned int _d; |
_d=va_arg(ap, unsigned int); |
_ulong = flags&SHORTINT ? (unsigned long)(unsigned short)_d : (unsigned long)_d; |
} |
} |
#ifndef LIGHTPRINTF |
if(ch==' ') { |
/* |
* ``If the space and + flags both appear, the space |
* flag will be ignored.'' |
* -- ANSI X3J11 |
*/ |
if (!sign) |
sign = ' '; |
goto rflag; |
} else if (ch=='#') { |
flags |= ALT; |
goto rflag; |
} else if (ch=='*'||ch=='-') { |
if (ch=='*') { |
/* |
* ``A negative field width argument is taken as a |
* - flag followed by a positive field width.'' |
* -- ANSI X3J11 |
* They don't exclude field widths read from args. |
*/ |
if ((width = va_arg(ap, int)) >= 0) |
goto rflag; |
width = -width; |
} |
flags |= LADJUST; |
flags &= ~ZEROPAD; /* '-' disables '0' */ |
goto rflag; |
} else if (ch=='+') { |
sign = '+'; |
goto rflag; |
} else if (ch=='.') { |
if ((ch = PRG_RDB(fmt++)) == '*') { |
n = va_arg(ap, int); |
prec = n < 0 ? -1 : n; |
goto rflag; |
} |
n = 0; |
while (is_digit(ch)) { |
n = n*10 + to_digit(ch); |
ch = PRG_RDB(fmt++); |
} |
prec = n < 0 ? -1 : n; |
goto reswitch; |
} else |
#endif /* LIGHTPRINTF */ |
if (ch=='0') { |
/* |
* ``Note that 0 is taken as a flag, not as the |
* beginning of a field width.'' |
* -- ANSI X3J11 |
*/ |
if (!(flags & LADJUST)) |
flags |= ZEROPAD; /* '-' disables '0' */ |
goto rflag; |
} else if (ch>='1' && ch<='9') { |
n = 0; |
do { |
n = 10 * n + to_digit(ch); |
ch = PRG_RDB(fmt++); |
} while (is_digit(ch)); |
width = n; |
goto reswitch; |
} else if (ch=='h') { |
flags |= SHORTINT; |
goto rflag; |
} else if (ch=='l') { |
flags |= LONGINT; |
goto rflag; |
} else if (ch=='c') { |
*(cp = buf) = va_arg(ap, int); |
size = 1; |
sign = '\0'; |
} else if (ch=='D'||ch=='d'||ch=='i') { |
if(ch=='D') |
flags |= LONGINT; |
if (flags&LONGINT) { |
_ulong=va_arg(ap, long); |
} else { |
register int _d; |
_d=va_arg(ap, int); |
_ulong = flags&SHORTINT ? (long)(short)_d : (long)_d; |
} |
if ((long)_ulong < 0) { |
_ulong = -_ulong; |
sign = '-'; |
} |
base = DEC; |
goto number; |
} else |
/* |
if (ch=='n') { |
if (flags & LONGINT) |
*va_arg(ap, long *) = ret; |
else if (flags & SHORTINT) |
*va_arg(ap, short *) = ret; |
else |
*va_arg(ap, int *) = ret; |
continue; // no output |
} else |
*/ |
#ifndef LIGHTPRINTF |
if (ch=='O'||ch=='o') { |
if (ch=='O') |
flags |= LONGINT; |
base = OCT; |
goto nosign; |
} else if (ch=='p') { |
/* |
* ``The argument shall be a pointer to void. The |
* value of the pointer is converted to a sequence |
* of printable characters, in an implementation- |
* defined manner.'' |
* -- ANSI X3J11 |
*/ |
/* NOSTRICT */ |
_ulong = (unsigned int)va_arg(ap, void *); |
base = HEX; |
flags |= HEXPREFIX; |
ch = 'x'; |
goto nosign; |
} else if (ch=='s') { // print a string from RAM |
if ((cp = va_arg(ap, char *)) == NULL) { |
cp=buf; |
cp[0] = '('; |
cp[1] = 'n'; |
cp[2] = 'u'; |
cp[4] = cp[3] = 'l'; |
cp[5] = ')'; |
cp[6] = '\0'; |
} |
if (prec >= 0) { |
/* |
* can't use strlen; can only look for the |
* NUL in the first `prec' characters, and |
* strlen() will go further. |
*/ |
char *p = (char*)memchr(cp, 0, prec); |
if (p != NULL) { |
size = p - cp; |
if (size > prec) |
size = prec; |
} else |
size = prec; |
} else |
size = strlen(cp); |
sign = '\0'; |
} else |
#endif /* LIGHTPRINTF */ |
if(ch=='U'||ch=='u') { |
if (ch=='U') |
flags |= LONGINT; |
base = DEC; |
goto nosign; |
} else if (ch=='X'||ch=='x') { |
base = HEX; |
/* leading 0x/X only if non-zero */ |
if (flags & ALT && _ulong != 0) |
flags |= HEXPREFIX; |
/* unsigned conversions */ |
nosign: sign = '\0'; |
/* |
* ``... diouXx conversions ... if a precision is |
* specified, the 0 flag will be ignored.'' |
* -- ANSI X3J11 |
*/ |
number: if ((dprec = prec) >= 0) |
flags &= ~ZEROPAD; |
/* |
* ``The result of converting a zero value with an |
* explicit precision of zero is no characters.'' |
* -- ANSI X3J11 |
*/ |
cp = buf + BUF; |
if (_ulong != 0 || prec != 0) { |
register unsigned char _d,notlastdigit; |
do { |
notlastdigit=(_ulong>=base); |
_d = _ulong % base; |
if (_d<10) { |
_d+='0'; |
} else { |
_d+='a'-10; |
if (ch=='X') _d&=~0x20; |
} |
*--cp=_d; |
_ulong /= base; |
} while (notlastdigit); |
#ifndef LIGHTPRINTF |
// handle octal leading 0 |
if (base==OCT && flags & ALT && *cp != '0') |
*--cp = '0'; |
#endif |
} |
size = buf + BUF - cp; |
} else { //default |
/* "%?" prints ?, unless ? is NUL */ |
if (ch == '\0') |
goto done; |
/* pretend it was %c with argument ch */ |
cp = buf; |
*cp = ch; |
size = 1; |
sign = '\0'; |
} |
/* |
* All reasonable formats wind up here. At this point, |
* `cp' points to a string which (if not flags&LADJUST) |
* should be padded out to `width' places. If |
* flags&ZEROPAD, it should first be prefixed by any |
* sign or other prefix; otherwise, it should be blank |
* padded before the prefix is emitted. After any |
* left-hand padding and prefixing, emit zeroes |
* required by a decimal [diouxX] precision, then print |
* the string proper, then emit zeroes required by any |
* leftover floating precision; finally, if LADJUST, |
* pad with blanks. |
*/ |
/* |
* compute actual size, so we know how much to pad. |
*/ |
fieldsz = size; |
dpad = dprec - size; |
if (dpad < 0) |
dpad = 0; |
if (sign) |
fieldsz++; |
else if (flags & HEXPREFIX) |
fieldsz += 2; |
fieldsz += dpad; |
/* right-adjusting blank padding */ |
if ((flags & (LADJUST|ZEROPAD)) == 0) |
PAD_SP(width - fieldsz); |
/* prefix */ |
if (sign) { |
PRINT(&sign, 1); |
} else if (flags & HEXPREFIX) { |
ox[0] = '0'; |
ox[1] = ch; |
PRINT(ox, 2); |
} |
/* right-adjusting zero padding */ |
if ((flags & (LADJUST|ZEROPAD)) == ZEROPAD) |
PAD_0(width - fieldsz); |
/* leading zeroes from decimal precision */ |
PAD_0(dpad); |
/* the string or number proper */ |
PRINT(cp, size); |
/* left-adjusting padding (always blank) */ |
if (flags & LADJUST) |
PAD_SP(width - fieldsz); |
} |
done: |
va_end(ap); |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/printf_P.h |
---|
0,0 → 1,17 |
#ifndef _PRINTF_P_H_ |
#define _PRINTF_P_H_ |
#include <avr/pgmspace.h> |
#define OUT_V24 0 |
#define OUT_LCD 1 |
void _printf_P (char, char const *fmt0, ...); |
extern char PrintZiel; |
#define printf_P(format, args...) _printf_P(OUT_V24,format , ## args) |
#define printf(format, args...) _printf_P(OUT_V24,PSTR(format) , ## args) |
#define LCD_printfxy(x,y,format, args...) { dispPtr = y * 20 + x; _printf_P(OUT_LCD,PSTR(format) , ## args);} |
#define LCD_printf(format, args...) { _printf_P(OUT_LCD,PSTR(format) , ## args);} |
#endif |
/branches/dongfang_FC_fixedwing/arduino_atmega328/rc.c |
---|
0,0 → 1,243 |
#include <stdlib.h> |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "rc.h" |
#include "controlMixer.h" |
#include "configuration.h" |
#include "commands.h" |
#include "output.h" |
// The channel array is 0-based! |
volatile int16_t PPM_in[MAX_CHANNELS]; |
volatile uint8_t RCQuality; |
uint8_t lastRCCommand = COMMAND_NONE; |
uint8_t lastFlightMode = FLIGHT_MODE_NONE; |
/*************************************************************** |
* 16bit timer 1 is used to decode the PPM-Signal |
***************************************************************/ |
void RC_Init(void) { |
uint8_t sreg = SREG; |
// disable all interrupts before reconfiguration |
cli(); |
// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1 |
DDRD &= ~(1<<6); |
PORTD |= (1<<PORTD6); |
// Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5) |
// set as output |
DDRD |= (1<<DDD5) | (1<<DDD4) | (1<<DDD3); |
// low level |
PORTD &= ~((1<<PORTD5) | (1<<PORTD4) | (1<<PORTD3)); |
// PD3 can't be used if 2nd UART is activated |
// because TXD1 is at that port |
// if (CPUType != ATMEGA644P) { |
// DDRD |= (1<<PORTD3); |
// PORTD &= ~(1<<PORTD3); |
// } |
// Timer/Counter1 Control Register A, B, C |
// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0) |
// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0) |
// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1) |
// Enable input capture noise cancler (bit: ICNC1=1) |
// Trigger on positive edge of the input capture pin (bit: ICES1=1), |
// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s |
// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s. |
TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) | (1 << WGM11) | (1 << WGM10)); |
TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12)); |
TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1); |
TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B)); |
// Timer/Counter1 Interrupt Mask Register |
// Enable Input Capture Interrupt (bit: ICIE1=1) |
// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0) |
// Enable Overflow Interrupt (bit: TOIE1=0) |
TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1)); |
TIMSK1 |= (1<<ICIE1); |
RCQuality = 0; |
SREG = sreg; |
} |
/********************************************************************/ |
/* Every time a positive edge is detected at PD6 */ |
/********************************************************************/ |
/* t-Frame |
<-----------------------------------------------------------------------> |
____ ______ _____ ________ ______ sync gap ____ |
| | | | | | | | | | | |
| | | | | | | | | | | |
___| |_| |_| |_| |_.............| |________________| |
<-----><-------><------><----------- <------> <--- |
t0 t1 t2 t4 tn t0 |
The PPM-Frame length is 22.5 ms. |
Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse. |
The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms. |
The maximum time delay of two events coding a channel is ( 1.7 + 0.3) ms = 2 ms. |
The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms. |
The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms. |
The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is |
the syncronization gap. |
*/ |
ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms |
int16_t signal = 0, tmp; |
static int16_t index; |
static uint16_t oldICR1 = 0; |
// 16bit Input Capture Register ICR1 contains the timer value TCNT1 |
// at the time the edge was detected |
// calculate the time delay to the previous event time which is stored in oldICR1 |
// calculatiing the difference of the two uint16_t and converting the result to an int16_t |
// implicit handles a timer overflow 65535 -> 0 the right way. |
signal = (uint16_t) ICR1 - oldICR1; |
oldICR1 = ICR1; |
//sync gap? (3.52 ms < signal < 25.6 ms) |
if ((signal > 1100) && (signal < 8000)) { |
index = 0; |
} else { // within the PPM frame |
if (index < MAX_CHANNELS) { // PPM24 supports 12 channels |
// check for valid signal length (0.8 ms < signal < 2.1984 ms) |
// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
if ((signal > 250) && (signal < 687)) { |
// shift signal to zero symmetric range -154 to 159 |
signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms) |
// check for stable signal |
if (abs(signal - PPM_in[index]) < 6) { |
if (RCQuality < 200) |
RCQuality += 10; |
else |
RCQuality = 200; |
} |
// If signal is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff. |
// if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) { |
// In addition, if the signal is very close to 0, just set it to 0. |
if (signal >= -1 && signal <= 1) { |
tmp = 0; |
//} else { |
// tmp = PPM_in[index]; |
// } |
} else |
tmp = signal; |
PPM_in[index] = tmp; // update channel value |
} |
index++; // next channel |
// demux sum signal for channels 5 to 7 to J3, J4, J5 |
// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the |
// channels are usually available at the receiver anyway. |
// if(index == 5) J3HIGH; else J3LOW; |
// if(index == 6) J4HIGH; else J4LOW; |
// if(CPUType != ATMEGA644P) // not used as TXD1 |
// { |
// if(index == 7) J5HIGH; else J5LOW; |
// } |
} |
} |
} |
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]] |
#define COMMAND_THRESHOLD 85 |
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
#define RC_SCALING 4 |
uint8_t getControlModeSwitch(void) { |
int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET; |
uint8_t flightMode = channel < 256/3 ? FLIGHT_MODE_MANUAL : |
(channel > 256*2/3 ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE); |
return flightMode; |
} |
// Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice. |
// Maybe simply: Very very low throttle. |
// Throttle xlow for COMMAND_TIMER: GYROCAL (once). |
// mode switched: CHMOD |
uint8_t RC_getCommand(void) { |
uint8_t flightMode = getControlModeSwitch(); |
if (lastFlightMode != flightMode) { |
lastFlightMode = flightMode; |
lastRCCommand = COMMAND_CHMOD; |
return lastRCCommand; |
} |
int16_t channel = RCChannel(CH_THROTTLE); |
if (channel <= -140) { // <= 900 us |
lastRCCommand = COMMAND_GYROCAL; |
} else { |
lastRCCommand = COMMAND_NONE; |
} |
return lastRCCommand; |
} |
uint8_t RC_getArgument(void) { |
return lastFlightMode; |
} |
/* |
* Get Pitch, Roll, Throttle, Yaw values |
*/ |
void RC_periodicTaskAndPRYT(int16_t* PRYT) { |
if (RCQuality) { |
RCQuality--; |
debugOut.analog[20] = RCChannel(CH_ELEVATOR); |
debugOut.analog[21] = RCChannel(CH_AILERONS); |
debugOut.analog[22] = RCChannel(CH_RUDDER); |
debugOut.analog[23] = RCChannel(CH_THROTTLE); |
PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) * RC_SCALING; |
PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) * RC_SCALING; |
PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) * RC_SCALING; |
PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) * RC_SCALING; |
} // if RCQuality is no good, we just do nothing. |
} |
/* |
* Get other channel value |
*/ |
int16_t RC_getVariable(uint8_t varNum) { |
if (varNum < 4) |
// 0th variable is 5th channel (1-based) etc. |
return RCChannel(varNum + CH_POTS) + POT_OFFSET; |
/* |
* Let's just say: |
* The RC variable i is hardwired to channel i, i>=4 |
*/ |
return PPM_in[varNum] + POT_OFFSET; |
} |
uint8_t RC_getSignalQuality(void) { |
if (RCQuality >= 160) |
return SIGNAL_GOOD; |
if (RCQuality >= 140) |
return SIGNAL_OK; |
if (RCQuality >= 120) |
return SIGNAL_BAD; |
return SIGNAL_LOST; |
} |
/* |
* To should fired only when the right stick is in the center position. |
* This will cause the value of pitch and roll stick to be adjusted |
* to zero (not just to near zero, as per the assumption in rc.c |
* about the rc signal. I had values about 50..70 with a Futaba |
* R617 receiver.) This calibration is not strictly necessary, but |
* for control logic that depends on the exact (non)center position |
* of a stick, it may be useful. |
*/ |
void RC_calibrate(void) { |
// Do nothing. |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/rc.h |
---|
0,0 → 1,45 |
#ifndef _RC_H |
#define _RC_H |
#include <inttypes.h> |
#include "configuration.h" |
// Number of cycles a command must be repeated before commit. |
#define COMMAND_TIMER 200 |
extern void RC_Init(void); |
// the RC-Signal. todo: Not export any more. |
extern volatile int16_t PPM_in[MAX_CHANNELS]; |
// extern volatile int16_t PPM_diff[MAX_CHANNELS]; // the differentiated RC-Signal. Should that be exported?? |
extern volatile uint8_t NewPpmData; // 0 indicates a new recieved PPM Frame |
extern volatile uint8_t RCQuality; // rc signal quality indicator (0 to 200) |
// defines for lookup staticParams.ChannelAssignment |
#define CH_ELEVATOR 0 |
#define CH_AILERONS 1 |
#define CH_THROTTLE 2 |
#define CH_RUDDER 3 |
#define CH_MODESWITCH 4 |
#define CH_POTS 4 |
#define POT_OFFSET 120 |
/* |
int16_t RC_getPitch (void); |
int16_t RC_getYaw (void); |
int16_t RC_getRoll (void); |
uint16_t RC_getThrottle (void); |
uint8_t RC_hasNewRCData (void); |
*/ |
// void RC_periodicTask(void); |
void RC_periodicTaskAndPRYT(int16_t* PRYT); |
uint8_t RC_getArgument(void); |
uint8_t RC_getCommand(void); |
int16_t RC_getVariable(uint8_t varNum); |
void RC_calibrate(void); |
uint8_t RC_getSignalQuality(void); |
uint8_t RC_getLooping(uint8_t looping); |
#ifdef USE_MK3MAG |
uint8_t RC_testCompassCalState(void); |
#endif |
#endif //_RC_H |
/branches/dongfang_FC_fixedwing/arduino_atmega328/sensors.h |
---|
0,0 → 1,27 |
#ifndef _SENSORS_H |
#define _SENSORS_H |
#include <inttypes.h> |
#include "configuration.h" |
extern sensorOffset_t gyroAmplifierOffset; |
/* |
* Common procedures for all gyro types. |
* FC 1.3 hardware: Searching the DAC values that return neutral readings. |
* FC 2.0 hardware: Nothing to do. |
* InvenSense hardware: Output a pulse on the AUTO_ZERO line. |
*/ |
void gyro_calibrate(void); |
/* |
* FC 1.3: Output data in gyroAmplifierOffset to DAC. All other versions: Do nothing. |
*/ |
void gyro_init(void); |
/* |
* Set some default FC parameters, depending on gyro type: Drift correction etc. |
*/ |
void gyro_setDefaultParameters(void); |
#endif |
/branches/dongfang_FC_fixedwing/arduino_atmega328/timer0.c |
---|
0,0 → 1,155 |
#include <inttypes.h> |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "eeprom.h" |
#include "analog.h" |
#include "controlMixer.h" |
#include "timer0.h" |
#include "output.h" |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#endif |
volatile uint32_t globalMillisClock = 0; |
volatile uint8_t runFlightControl = 0; |
volatile uint16_t beepTime = 0; |
volatile uint16_t beepModulation = BEEP_MODULATION_NONE; |
#ifdef USE_NAVICTRL |
volatile uint8_t SendSPI = 0; |
#endif |
/***************************************************** |
* Initialize Timer 0 |
*****************************************************/ |
// timer 0 is used for the PWM generation to control the offset voltage at the air pressure sensor |
// Its overflow interrupt routine is used to generate the beep signal and the flight control motor update rate |
void timer0_init(void) { |
uint8_t sreg = SREG; |
// disable all interrupts before reconfiguration |
cli(); |
// Configure speaker port as output. |
DDRD |= (1 << DDD5); |
PORTD &= ~(1 << PORTD5); |
// Timer/Counter 0 Control Register A |
// Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1) |
// Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0) |
// Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0) |
// TCCR0A &= ~((1 << COM0A0) | (1 << COM0B0)); |
// TCCR0A |= (1 << COM0A1) | (1 << COM0B1) | (1 << WGM01) | (1 << WGM00); |
// Timer/Counter 0 Control Register B |
// set clock divider for timer 0 to SYSCLOCK/8 = 20MHz/8 = 2.5MHz |
// i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz |
// hence the timer overflow interrupt frequency is 2.5 MHz/256 = 9.765 kHz |
// divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0) |
TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02)); |
TCCR0B = (TCCR0B & 0xF8) | (0 << CS02) | (1 << CS01) | (0 << CS00); |
// initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4 |
OCR0A = 0; // for PB3 |
OCR0B = 120; // for PB4 |
// init Timer/Counter 0 Register |
TCNT0 = 0; |
// Timer/Counter 0 Interrupt Mask Register |
// enable timer overflow interrupt only |
TIMSK0 &= ~((1 << OCIE0B) | (1 << OCIE0A)); |
TIMSK0 |= (1 << TOIE0); |
SREG = sreg; |
} |
/*****************************************************/ |
/* Interrupt Routine of Timer 0 */ |
/*****************************************************/ |
ISR(TIMER0_OVF_vect) { // 9765.625 Hz |
static uint8_t cnt_1ms = 1, cnt = 0; |
uint8_t beeperOn = 0; |
#ifdef USE_NAVICTRL |
if(SendSPI) SendSPI--; // if SendSPI is 0, the transmit of a byte via SPI bus to and from The Navicontrol is done |
#endif |
if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz) |
cnt = 9; |
cnt_1ms ^= 1; |
if (!cnt_1ms) { |
if (runFlightControl == 1) |
debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |
else |
debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz) |
} |
globalMillisClock++; // increment millisecond counter |
} |
// beeper on if duration is not over |
if (beepTime) { |
beepTime--; // decrement BeepTime |
if (beepTime & beepModulation) |
beeperOn = 1; |
else |
beeperOn = 0; |
} else { // beeper off if duration is over |
beeperOn = 0; |
beepModulation = BEEP_MODULATION_NONE; |
} |
if (beeperOn) { |
// set speaker port to high. |
PORTD |= (1 << PORTD5); // Speaker at PD5 |
} else { // beeper is off |
// set speaker port to low |
PORTD &= ~(1 << PORTD5);// Speaker at PD5 |
} |
#ifdef USE_MK3MAG |
// update compass value if this option is enabled in the settings |
if (staticParams.bitConfig & CFG_COMPASS_ENABLED) { |
MK3MAG_periodicTask(); // read out mk3mag pwm |
} |
#endif |
} |
// ----------------------------------------------------------------------- |
uint16_t setDelay(uint16_t t) { |
return (globalMillisClock + t - 1); |
} |
// ----------------------------------------------------------------------- |
int8_t checkDelay(uint16_t t) { |
return (((t - globalMillisClock) & 0x8000) >> 8); // check sign bit |
} |
// ----------------------------------------------------------------------- |
void delay_ms(uint16_t w) { |
uint16_t t_stop = setDelay(w); |
while (!checkDelay(t_stop)) |
; |
} |
// ----------------------------------------------------------------------- |
void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop) { |
uint16_t t_stop; |
t_stop = setDelay(w); |
while (!checkDelay(t_stop)) { |
if (sensorDataReady == ALL_DATA_READY) { |
analog_update(); |
startAnalogConversionCycle(); |
} |
} |
if (stop) { |
// Wait for new samples to get prepared but do not restart AD conversion after that! |
// Caller MUST to that. |
while (!sensorDataReady != ALL_DATA_READY); |
} |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/timer0.h |
---|
0,0 → 1,32 |
#ifndef _TIMER0_H |
#define _TIMER0_H |
#include <inttypes.h> |
// Normally it is 20MHz/2048 = 9765.625 Hz |
#define F_TIMER0IRQ ((float)F_CPU/2048.0) |
#define MAINLOOP_DIVIDER 10 |
// Originally it is 488 |
#define F_MAINLOOP (F_TIMER0IRQ/(2.0*MAINLOOP_DIVIDER)) |
#define BEEP_MODULATION_NONE 0xFFFF |
#define BEEP_MODULATION_RCALARM 0x0C00 |
#define BEEP_MODULATION_I2CALARM 0x0080 |
#define BEEP_MODULATION_BATTERYALARM 0x0300 |
#define BEEP_MODULATION_EEPROMALARM 0x0007 |
extern volatile uint32_t globalMillisClock; |
extern volatile uint8_t runFlightControl; |
extern volatile uint16_t beepModulation; |
extern volatile uint16_t beepTime; |
#ifdef USE_NAVICTRL |
extern volatile uint8_t SendSPI; |
#endif |
extern void timer0_init(void); |
extern void delay_ms(uint16_t w); |
extern void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop); |
extern uint16_t setDelay(uint16_t t); |
extern int8_t checkDelay(uint16_t t); |
#endif //_TIMER0_H |
/branches/dongfang_FC_fixedwing/arduino_atmega328/timer2.c |
---|
0,0 → 1,199 |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "eeprom.h" |
#include "output.h" |
#include "flight.h" |
#include "attitude.h" |
#include "timer2.h" |
// #define COARSERESOLUTION 1 |
#ifdef COARSERESOLUTION |
#define NEUTRAL_PULSELENGTH 938 |
#define STABILIZATION_LOG_DIVIDER 6 |
#define SERVOLIMIT 500 |
#define SCALE_FACTOR 4 |
#define CS2 ((1<<CS21)|(1<<CS20)) |
#else |
#define NEUTRAL_PULSELENGTH 3750 |
#define STABILIZATION_LOG_DIVIDER 4 |
#define SERVOLIMIT 2000 |
#define SCALE_FACTOR 16 |
#define CS2 (1<<CS21) |
#endif |
#define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128) |
#define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
#define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
volatile uint8_t recalculateServoTimes = 0; |
volatile uint16_t servoValues[MAX_SERVOS]; |
volatile uint16_t previousManualValues[2]; |
#define HEF4017R_ON PORTD |= (1<<PORTD4) |
#define HEF4017R_OFF PORTD &= ~(1<<PORTD4) |
/***************************************************** |
* Initialize Timer 2 |
*****************************************************/ |
void timer2_init(void) { |
uint8_t sreg = SREG; |
// disable all interrupts before reconfiguration |
cli(); |
// set PD7 as output of the PWM for pitch servo |
DDRD |= (1 << DDD3); |
PORTD &= ~(1 << PORTD3); // set PD7 to low |
DDRD |= (1 << DDD4); // set PC6 as output (Reset for HEF4017) |
HEF4017R_ON; // enable reset |
// Timer/Counter 2 Control Register A |
// Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
// PD3: Output OCR2 match, (Bits: COM2B1 = 1, COM2B0 = 0) |
// PB3: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0) |
TCCR2A &= ~((1 << COM2B0) | (1 << COM2A1) | (1 << COM2A0) | (1 << WGM20) | (1 << WGM22)); |
TCCR2A |= (1 << COM2B1) | (1 << WGM21); |
// Timer/Counter 2 Control Register B |
// Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz |
// The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us |
// hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms |
TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
TCCR2B |= CS2; |
// Initialize the Timer/Counter 2 Register |
TCNT2 = 0; |
// Initialize the Output Compare Register A used for signal generation on port PD7. |
OCR2A = 255; |
// Timer/Counter 2 Interrupt Mask Register |
// Enable timer output compare match A Interrupt only |
TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
TIMSK2 |= (1 << OCIE2A); |
for (uint8_t axis=0; axis<2; axis++) |
previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR; |
SREG = sreg; |
} |
/***************************************************** |
* Control (camera gimbal etc.) servos |
*****************************************************/ |
int16_t calculateStabilizedServoAxis(uint8_t axis) { |
int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about |
// With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
// That is a divisor of about 1<<14. Same conclusion as H&I. |
value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
value = value >> 8; |
if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
return -value; |
return value; |
} |
// With constant-speed limitation. |
uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
int16_t diff = manualValue - previousManualValues[axis]; |
uint8_t maxSpeed = staticParams.servoManualMaxSpeed; |
if (diff > maxSpeed) diff = maxSpeed; |
else if (diff < -maxSpeed) diff = -maxSpeed; |
manualValue = previousManualValues[axis] + diff; |
previousManualValues[axis] = manualValue; |
return manualValue; |
} |
// add stabilization and manual, apply soft position limits. |
// All in a [0..255*SCALE_FACTOR] space (despite signed types used internally) |
int16_t featuredServoValue(uint8_t axis) { |
int16_t value = calculateManualServoAxis(axis, dynamicParams.servoManualControl[axis] * SCALE_FACTOR); |
value += calculateStabilizedServoAxis(axis); |
int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR; |
if (value < limit) value = limit; |
limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
if (value > limit) value = limit; |
value -= (128 * SCALE_FACTOR); |
if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
else if (value > SERVOLIMIT) value = SERVOLIMIT; |
// Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
return value + NEUTRAL_PULSELENGTH; |
} |
void calculateControlServoValues(void) { |
int16_t value; |
for (uint8_t axis=0; axis<4; axis++) { |
value = controlServos[axis]; |
value *= 2; |
servoValues[axis] = value + NEUTRAL_PULSELENGTH; |
} |
debugOut.analog[24] = servoValues[0]; |
debugOut.analog[25] = servoValues[1]; |
debugOut.analog[26] = servoValues[2]; |
debugOut.analog[27] = servoValues[3]; |
} |
void calculateFeaturedServoValues(void) { |
int16_t value; |
uint8_t axis; |
// Save the computation cost of computing a new value before the old one is used. |
if (!recalculateServoTimes) return; |
for (axis= MAX_CONTROL_SERVOS; axis<MAX_CONTROL_SERVOS+2; axis++) { |
value = featuredServoValue(axis-MAX_CONTROL_SERVOS); |
servoValues[axis] = value; |
} |
for (axis=MAX_CONTROL_SERVOS+2; axis<MAX_SERVOS; axis++) { |
value = 128 * SCALE_FACTOR; |
servoValues[axis] = value; |
} |
recalculateServoTimes = 0; |
} |
ISR(TIMER2_COMPA_vect) { |
static uint16_t remainingPulseTime; |
static uint8_t servoIndex = 0; |
static uint16_t sumOfPulseTimes = 0; |
if (!remainingPulseTime) { |
// Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
if (servoIndex < staticParams.servoCount) { |
// There are more signals to output. |
sumOfPulseTimes += (remainingPulseTime = servoValues[servoIndex]); |
servoIndex++; |
} else { |
// There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
sumOfPulseTimes = servoIndex = 0; |
recalculateServoTimes = 1; |
HEF4017R_ON; |
} |
} |
// Schedule the next OCR2A event. The counter is already reset at this time. |
if (remainingPulseTime > 256+128) { |
// Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
// as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
OCR2A = 255; |
TCCR2A &= ~(1<<COM2A0); |
remainingPulseTime-=256; |
} else if (remainingPulseTime > 256) { |
// Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard |
// way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt |
// condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
remainingPulseTime-=128; |
OCR2A=127; |
} else { |
// Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low |
TCCR2A |= (1<<COM2A0); |
OCR2A = remainingPulseTime-1; |
remainingPulseTime=0; |
HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
} |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/timer2.h |
---|
0,0 → 1,14 |
#ifndef _TIMER2_H |
#define _TIMER2_H |
#include <inttypes.h> |
#define MAX_SERVOS 8 |
#define MAX_CONTROL_SERVOS 4 |
void calculateControlServoValues(void); |
void calculateFeaturedServoValues(void); |
void timer2_init(void); |
#endif //_TIMER2_H |
/branches/dongfang_FC_fixedwing/arduino_atmega328/twimaster.c |
---|
0,0 → 1,150 |
#include "twimaster.h" |
#include "analog.h" |
#include <inttypes.h> |
#include <util/twi.h> |
#include <avr/interrupt.h> |
uint8_t twiState; |
uint8_t twiTroubleSpot; |
uint8_t twiGotStatus; |
volatile uint16_t IMU3200SensorInputs[4]; |
void twimaster_init(void) { |
// Set pullups |
uint8_t sreg = SREG; |
cli(); |
DDRC = 0; |
PORTC = ((1 << 4) | (1 << 5)); |
TWBR = ((F_CPU / SCL_CLOCK) - 16) / 2; |
twiState = TWI_STATE_INIT_0; |
SREG = sreg; |
} |
void I2CStart(void) { |
TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWIE); |
} |
void I2CStop(void) { |
TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); |
} |
void I2CWriteByte(int8_t byte) { |
TWDR = byte; |
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
} |
void I2CReceiveByte(void) { |
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE) | (1 << TWEA); |
} |
void I2CReceiveLastByte(void) { |
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE); |
} |
void I2CReset(void) { |
I2CStop(); |
TWCR = (1 << TWINT); // reset to original state incl. interrupt flag reset |
TWAMR = 0; |
TWAR = 0; |
TWDR = 0; |
TWSR = 0; |
TWBR = 0; |
// Naaaah we dont need this: init_I2C(); |
I2CStart(); |
twiState = TWI_STATE_INIT_0; |
} |
void twimaster_setNeutral(void) { |
twiState = TWI_STATE_INIT_0; |
I2CStart(); |
} |
void twimaster_startCycle(void) { |
twiState = TWI_STATE_LOOP_0; |
I2CStart(); |
} |
const uint8_t EXPECTED_STATUS[] = { START, MT_SLA_ACK, MT_DATA_ACK, |
MT_DATA_ACK, MT_DATA_ACK, REPEATED_START, MT_SLA_ACK, MT_DATA_ACK, |
REPEATED_START, MR_SLA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK, |
MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_ACK, MR_DATA_NACK }; |
const uint8_t GYRO_CONFIGURATION[] = { 15, 3 << 3 }; |
ISR( TWI_vect) { |
if (twiState == sizeof(EXPECTED_STATUS)) { |
// We have come too far, panic! |
I2CReset(); |
} |
uint8_t cur_twi_state = twiState; |
twiState++; |
uint8_t status = TWSR_FILTER; |
if (status != EXPECTED_STATUS[cur_twi_state]) { |
// A little confusion between repeated and nonrepeated start is OK. |
if (!((status == START && EXPECTED_STATUS[cur_twi_state] |
== REPEATED_START) || (status == REPEATED_START |
&& EXPECTED_STATUS[cur_twi_state] == START))) { |
// Panic, reset |
twiTroubleSpot = cur_twi_state; |
twiGotStatus = status; |
I2CReset(); |
} |
} |
uint8_t dataIndex = cur_twi_state - TWI_STATE_LOOP_0 - 5; |
// Fix endiannness! |
if (dataIndex & 1) |
dataIndex &= ~1; |
else |
dataIndex |= 1; |
switch (cur_twi_state) { |
case TWI_STATE_INIT_0: |
I2CWriteByte((SLA << 1) | 0); |
break; |
case TWI_STATE_INIT_0 + 1: |
I2CWriteByte(GYRO_CONFIGURATION_START); |
break; |
case TWI_STATE_INIT_0 + 2: |
I2CWriteByte(GYRO_CONFIGURATION[0]); |
break; |
case TWI_STATE_INIT_0 + 3: |
I2CWriteByte(GYRO_CONFIGURATION[1]); |
break; |
case TWI_STATE_INIT_0 + 4: |
I2CStop(); |
// Need to check ACK ans return to 0 if not (skipping start below). |
I2CStart(); |
break; |
case TWI_STATE_LOOP_0: |
I2CWriteByte((SLA << 1) | 0); |
break; |
case TWI_STATE_LOOP_0 + 1: |
I2CWriteByte(GYRO_DATA_START); |
break; |
case TWI_STATE_LOOP_0 + 2: |
I2CStop(); |
I2CStart(); |
break; |
case TWI_STATE_LOOP_0 + 3: |
I2CWriteByte((SLA << 1) | 1); |
break; |
case TWI_STATE_LOOP_0 + 4: |
I2CReceiveByte(); |
break; |
default: // data bytes |
((uint8_t*) IMU3200SensorInputs)[dataIndex] = TWDR; |
if (twiState < TWI_STATE_LOOP_0 + 5 + sizeof(IMU3200SensorInputs) - 1) |
I2CReceiveByte(); |
else |
I2CReceiveLastByte(); |
break; |
case TWI_STATE_LOOP_0 + 5 + sizeof(IMU3200SensorInputs) - 1: // last data byte |
((uint8_t*) IMU3200SensorInputs)[dataIndex] = TWDR; |
// Dont re-init the gyro but just restart the loop. |
I2CStop(); |
sensorDataReady |= TWI_DATA_READY; |
break; |
} |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/twimaster.h |
---|
0,0 → 1,37 |
#ifndef _TWIMASTER_H |
#define _TWIMASTER_H |
#include <inttypes.h> |
// Cold start, (re)write configuration. |
#define TWI_STATE_INIT_0 0 |
// Start the read-data loop. |
#define TWI_STATE_LOOP_0 5 |
#define SCL_CLOCK 400000L |
// This is for the IMU3200 sensor. |
#define SLA 0b1101001 |
#define TWSR_FILTER (TWSR & 0b11111100) |
#define GYRO_CONFIGURATION_START 0x15 |
#define GYRO_DATA_START 0x1B |
#define START 0x08 |
#define REPEATED_START 0x10 |
#define MT_SLA_ACK 0x18 |
#define MT_SLA_NACK 0x20 |
#define MT_DATA_ACK 0x28 |
#define MR_SLA_ACK 0x40 |
#define MR_DATA_ACK 0x50 |
#define MR_DATA_NACK 0x58 |
/* |
* We take the temperature measurement as well as gyro. |
*/ |
extern volatile uint16_t IMU3200SensorInputs[4]; |
void twimaster_setNeutral(void); |
void twimaster_startCycle(void); |
#endif |
/branches/dongfang_FC_fixedwing/arduino_atmega328/uart0.c |
---|
0,0 → 1,686 |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include <avr/wdt.h> |
#include <avr/pgmspace.h> |
#include <stdarg.h> |
#include <string.h> |
#include "eeprom.h" |
#include "timer0.h" |
#include "uart0.h" |
#include "rc.h" |
#include "externalControl.h" |
#include "output.h" |
#include "attitude.h" |
#include "commands.h" |
#define FC_ADDRESS 1 |
#define NC_ADDRESS 2 |
#define MK3MAG_ADDRESS 3 |
#define FALSE 0 |
#define TRUE 1 |
int8_t displayBuff[DISPLAYBUFFSIZE]; |
uint8_t dispPtr = 0; |
uint8_t requestedDebugLabel = 255; |
uint8_t request_verInfo = FALSE; |
uint8_t request_externalControl = FALSE; |
uint8_t request_debugData = FALSE; |
uint8_t request_data3D = FALSE; |
uint8_t request_PPMChannels = FALSE; |
uint8_t request_servoTest = FALSE; |
uint8_t request_variables = FALSE; |
uint8_t request_OSD = FALSE; |
/* |
#define request_verInfo (1<<0) |
#define request_externalControl (1<<1) |
#define request_display (1<<3) |
#define request_display1 (1<<4) |
#define request_debugData (1<<5) |
#define request_data3D (1<<6) |
#define request_PPMChannels (1<<7) |
#define request_motorTest (1<<8) |
#define request_variables (1<<9) |
#define request_OSD (1<<10) |
*/ |
//uint16_t request = 0; |
volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
volatile uint8_t rxd_buffer_locked = FALSE; |
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
volatile uint8_t txd_complete = TRUE; |
volatile uint8_t receivedBytes = 0; |
volatile uint8_t *pRxData = 0; |
volatile uint8_t rxDataLen = 0; |
uint8_t servoTestActive = 0; |
uint8_t servoTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; |
uint8_t confirmFrame; |
typedef struct { |
int16_t heading; |
}__attribute__((packed)) Heading_t; |
Data3D_t data3D; |
uint16_t debugData_timer; |
uint16_t data3D_timer; |
uint16_t OSD_timer; |
uint16_t debugData_interval = 0; // in 1ms |
uint16_t data3D_interval = 0; // in 1ms |
uint16_t OSD_interval = 0; |
#ifdef USE_DIRECT_GPS |
int16_t toMk3MagTimer; |
#endif |
// keep lables in flash to save 512 bytes of sram space |
const prog_uint8_t ANALOG_LABEL[32][16] = { |
//1234567890123456 |
"Gyro P ", //0 |
"Gyro R ", |
"Gyro Y ", |
"Attitude P ", |
"Attitude R ", |
"Attitude Y ", //5 |
"Target P ", |
"Target R ", |
"Target Y ", |
"Error P ", |
"Error R ", //10 |
"Error Y ", |
"Term P ", |
"Term R ", |
"Throttle ", |
"Term Y ", //15 |
"Flight mode ", |
" ", |
" ", |
" ", |
"RC P ", //20 |
"RC R ", |
"RC T ", |
"RC Y ", |
"Servo P ", |
"Servo R ", //25 |
"Servo T ", |
"Servo Y ", |
" ", |
" . ", |
" ", //30 |
" " |
}; |
/****************************************************************/ |
/* Initialization of the USART0 */ |
/****************************************************************/ |
void usart0_init(void) { |
uint8_t sreg = SREG; |
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK / (8 * USART0_BAUD) - 1); |
// disable all interrupts before configuration |
cli(); |
// disable RX-Interrupt |
UCSR0B &= ~(1 << RXCIE0); |
// disable TX-Interrupt |
UCSR0B &= ~(1 << TXCIE0); |
// set direction of RXD0 and TXD0 pins |
// set RXD0 (PD0) as an input pin |
PORTD |= (1 << PORTD0); |
DDRD &= ~(1 << DDD0); |
// set TXD0 (PD1) as an output pin |
PORTD |= (1 << PORTD1); |
DDRD |= (1 << DDD1); |
// USART0 Baud Rate Register |
// set clock divider |
UBRR0H = (uint8_t) (ubrr >> 8); |
UBRR0L = (uint8_t) ubrr; |
// USART0 Control and Status Register A, B, C |
// enable double speed operation in |
UCSR0A |= (1 << U2X0); |
// enable receiver and transmitter in |
UCSR0B = (1 << TXEN0) | (1 << RXEN0); |
// set asynchronous mode |
UCSR0C &= ~(1 << UMSEL01); |
UCSR0C &= ~(1 << UMSEL00); |
// no parity |
UCSR0C &= ~(1 << UPM01); |
UCSR0C &= ~(1 << UPM00); |
// 1 stop bit |
UCSR0C &= ~(1 << USBS0); |
// 8-bit |
UCSR0B &= ~(1 << UCSZ02); |
UCSR0C |= (1 << UCSZ01); |
UCSR0C |= (1 << UCSZ00); |
// flush receive buffer |
while (UCSR0A & (1 << RXC0)) |
UDR0; |
// enable interrupts at the end |
// enable RX-Interrupt |
UCSR0B |= (1 << RXCIE0); |
// enable TX-Interrupt |
UCSR0B |= (1 << TXCIE0); |
// initialize the debug timer |
debugData_timer = setDelay(debugData_interval); |
// unlock rxd_buffer |
rxd_buffer_locked = FALSE; |
pRxData = 0; |
rxDataLen = 0; |
// no bytes to send |
txd_complete = TRUE; |
#ifdef USE_DIRECT_GPS |
toMk3MagTimer = setDelay(220); |
#endif |
versionInfo.SWMajor = VERSION_MAJOR; |
versionInfo.SWMinor = VERSION_MINOR; |
versionInfo.SWPatch = VERSION_PATCH; |
versionInfo.protoMajor = VERSION_SERIAL_MAJOR; |
versionInfo.protoMinor = VERSION_SERIAL_MINOR; |
// restore global interrupt flags |
SREG = sreg; |
} |
/****************************************************************/ |
/* USART0 transmitter ISR */ |
/****************************************************************/ |
ISR(USART_TX_vect) { |
static uint16_t ptr_txd_buffer = 0; |
uint8_t tmp_tx; |
if (!txd_complete) { // transmission not completed |
ptr_txd_buffer++; // die [0] wurde schon gesendet |
tmp_tx = txd_buffer[ptr_txd_buffer]; |
// if terminating character or end of txd buffer was reached |
if ((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN)) { |
ptr_txd_buffer = 0; // reset txd pointer |
txd_complete = 1; // stop transmission |
} |
UDR0 = tmp_tx; // send current byte will trigger this ISR again |
} |
// transmission completed |
else |
ptr_txd_buffer = 0; |
} |
/****************************************************************/ |
/* USART0 receiver ISR */ |
/****************************************************************/ |
ISR(USART_RX_vect) { |
static uint16_t checksum; |
static uint8_t ptr_rxd_buffer = 0; |
uint8_t checksum1, checksum2; |
uint8_t c; |
c = UDR0; // catch the received byte |
if (rxd_buffer_locked) |
return; // if rxd buffer is locked immediately return |
// the rxd buffer is unlocked |
if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received |
rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer |
checksum = c; // init checksum |
} |
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes |
if (c != '\r') { // no termination character |
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
checksum += c; // update checksum |
} else { // termination character was received |
// the last 2 bytes are no subject for checksum calculation |
// they are the checksum itself |
checksum -= rxd_buffer[ptr_rxd_buffer - 2]; |
checksum -= rxd_buffer[ptr_rxd_buffer - 1]; |
// calculate checksum from transmitted data |
checksum %= 4096; |
checksum1 = '=' + checksum / 64; |
checksum2 = '=' + checksum % 64; |
// compare checksum to transmitted checksum bytes |
if ((checksum1 == rxd_buffer[ptr_rxd_buffer - 2]) && (checksum2 |
== rxd_buffer[ptr_rxd_buffer - 1])) { |
// checksum valid |
rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
receivedBytes = ptr_rxd_buffer + 1;// store number of received bytes |
rxd_buffer_locked = TRUE; // lock the rxd buffer |
// if 2nd byte is an 'R' enable watchdog that will result in an reset |
if (rxd_buffer[2] == 'R') { |
wdt_enable(WDTO_250MS); |
} // Reset-Commando |
} else { // checksum invalid |
rxd_buffer_locked = FALSE; // unlock rxd buffer |
} |
ptr_rxd_buffer = 0; // reset rxd buffer pointer |
} |
} else { // rxd buffer overrun |
ptr_rxd_buffer = 0; // reset rxd buffer |
rxd_buffer_locked = FALSE; // unlock rxd buffer |
} |
} |
// -------------------------------------------------------------------------- |
void addChecksum(uint16_t datalen) { |
uint16_t tmpchecksum = 0, i; |
for (i = 0; i < datalen; i++) { |
tmpchecksum += txd_buffer[i]; |
} |
tmpchecksum %= 4096; |
txd_buffer[i++] = '=' + (tmpchecksum >> 6); |
txd_buffer[i++] = '=' + (tmpchecksum & 0x3F); |
txd_buffer[i++] = '\r'; |
txd_complete = FALSE; |
UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
} |
// -------------------------------------------------------------------------- |
// application example: |
// sendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16); |
/* |
void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
va_list ap; |
uint16_t txd_bufferIndex = 0; |
uint8_t *currentBuffer; |
uint8_t currentBufferIndex; |
uint16_t lengthOfCurrentBuffer; |
uint8_t shift = 0; |
txd_buffer[txd_bufferIndex++] = '#'; // Start character |
txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...) |
txd_buffer[txd_bufferIndex++] = cmd; // Command |
va_start(ap, numofbuffers); |
while(numofbuffers) { |
currentBuffer = va_arg(ap, uint8_t*); |
lengthOfCurrentBuffer = va_arg(ap, int); |
currentBufferIndex = 0; |
// Encode data: 3 bytes of data are encoded into 4 bytes, |
// where the 2 most significant bits are both 0. |
while(currentBufferIndex != lengthOfCurrentBuffer) { |
if (!shift) txd_buffer[txd_bufferIndex] = 0; |
txd_buffer[txd_bufferIndex] |= currentBuffer[currentBufferIndex] >> (shift + 2); |
txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111; |
shift += 2; |
if (shift == 6) { shift=0; txd_bufferIndex++; } |
currentBufferIndex++; |
} |
} |
// If the number of data bytes was not divisible by 3, stuff |
// with 0 pseudodata until length is again divisible by 3. |
if (shift == 2) { |
// We need to stuff with zero bytes at the end. |
txd_buffer[txd_bufferIndex] &= 0b00110000; |
txd_buffer[++txd_bufferIndex] = 0; |
shift = 4; |
} |
if (shift == 4) { |
// We need to stuff with zero bytes at the end. |
txd_buffer[txd_bufferIndex++] &= 0b00111100; |
txd_buffer[txd_bufferIndex] = 0; |
} |
va_end(ap); |
Addchecksum(pt); // add checksum after data block and initates the transmission |
} |
*/ |
void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
va_list ap; |
uint16_t pt = 0; |
uint8_t a, b, c; |
uint8_t ptr = 0; |
uint8_t *pdata = 0; |
int len = 0; |
txd_buffer[pt++] = '#'; // Start character |
txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...) |
txd_buffer[pt++] = cmd; // Command |
va_start(ap, numofbuffers); |
if (numofbuffers) { |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
while (len) { |
if (len) { |
a = pdata[ptr++]; |
len--; |
if ((!len) && numofbuffers) { |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} else |
a = 0; |
if (len) { |
b = pdata[ptr++]; |
len--; |
if ((!len) && numofbuffers) { |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} else |
b = 0; |
if (len) { |
c = pdata[ptr++]; |
len--; |
if ((!len) && numofbuffers) { |
pdata = va_arg(ap, uint8_t*); |
len = va_arg(ap, int); |
ptr = 0; |
numofbuffers--; |
} |
} else |
c = 0; |
txd_buffer[pt++] = '=' + (a >> 2); |
txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
txd_buffer[pt++] = '=' + (c & 0x3f); |
} |
va_end(ap); |
addChecksum(pt); // add checksum after data block and initates the transmission |
} |
// -------------------------------------------------------------------------- |
void decode64(void) { |
uint8_t a, b, c, d; |
uint8_t x, y, z; |
uint8_t ptrIn = 3; |
uint8_t ptrOut = 3; |
uint8_t len = receivedBytes - 6; |
while (len) { |
a = rxd_buffer[ptrIn++] - '='; |
b = rxd_buffer[ptrIn++] - '='; |
c = rxd_buffer[ptrIn++] - '='; |
d = rxd_buffer[ptrIn++] - '='; |
//if(ptrIn > ReceivedBytes - 3) break; |
x = (a << 2) | (b >> 4); |
y = ((b & 0x0f) << 4) | (c >> 2); |
z = ((c & 0x03) << 6) | d; |
if (len--) |
rxd_buffer[ptrOut++] = x; |
else |
break; |
if (len--) |
rxd_buffer[ptrOut++] = y; |
else |
break; |
if (len--) |
rxd_buffer[ptrOut++] = z; |
else |
break; |
} |
pRxData = &rxd_buffer[3]; |
rxDataLen = ptrOut - 3; |
} |
// -------------------------------------------------------------------------- |
void usart0_processRxData(void) { |
// We control the servoTestActive var from here: Count it down. |
if (servoTestActive) |
servoTestActive--; |
// if data in the rxd buffer are not locked immediately return |
if (!rxd_buffer_locked) |
return; |
uint8_t tempchar[3]; |
decode64(); // decode data block in rxd_buffer |
switch (rxd_buffer[1] - 'a') { |
case FC_ADDRESS: |
switch (rxd_buffer[2]) { |
#ifdef USE_DIRECT_GPS |
case 'K':// compass value |
// What is the point of this - the compass will overwrite this soon? |
magneticHeading = ((Heading_t *)pRxData)->heading; |
// compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
break; |
#endif |
case 't': // motor test |
if (rxDataLen > 20) { |
memcpy(&servoTest[0], (uint8_t*) pRxData, sizeof(servoTest)); |
} else { |
memcpy(&servoTest[0], (uint8_t*) pRxData, 4); |
} |
servoTestActive = 255; |
externalControlActive = 255; |
break; |
case 'p': // get PPM channels |
request_PPMChannels = TRUE; |
break; |
case 'i':// Read IMU configuration |
tempchar[0] = IMUCONFIG_REVISION; |
tempchar[1] = sizeof(IMUConfig); |
while (!txd_complete) |
; // wait for previous frame to be sent |
sendOutData('I', FC_ADDRESS, 2, &tempchar, 2, (uint8_t *) &IMUConfig, sizeof(IMUConfig)); |
break; |
case 'j':// Save IMU configuration |
if (!isFlying) // save settings only if motors are off |
{ |
if ((pRxData[0] == IMUCONFIG_REVISION) && (pRxData[1] == sizeof(IMUConfig))) { |
memcpy(&IMUConfig, (uint8_t*) &pRxData[2], sizeof(IMUConfig)); |
IMUConfig_writeToEEprom(); |
tempchar[0] = 1; //indicate ok data |
} else { |
tempchar[0] = 0; //indicate bad data |
} |
while (!txd_complete) |
; // wait for previous frame to be sent |
sendOutData('J', FC_ADDRESS, 1, &tempchar, 1); |
} |
break; |
case 'q':// request settings |
if (pRxData[0] == 0xFF) { |
pRxData[0] = getParamByte(PID_ACTIVE_SET); |
} |
// limit settings range |
if (pRxData[0] < 1) |
pRxData[0] = 1; // limit to 1 |
else if (pRxData[0] > 5) |
pRxData[0] = 5; // limit to 5 |
// load requested parameter set |
paramSet_readFromEEProm(pRxData[0]); |
tempchar[0] = pRxData[0]; |
tempchar[1] = EEPARAM_REVISION; |
tempchar[2] = sizeof(staticParams); |
while (!txd_complete) |
; // wait for previous frame to be sent |
sendOutData('Q', FC_ADDRESS, 2, &tempchar, 3, (uint8_t *) &staticParams, sizeof(staticParams)); |
break; |
case 's': // save settings |
if (!isFlying) // save settings only if motors are off |
{ |
if ((pRxData[1] == EEPARAM_REVISION) && (pRxData[2] == sizeof(staticParams))) // check for setting to be in range and version of settings |
{ |
memcpy(&staticParams, (uint8_t*) &pRxData[3], sizeof(staticParams)); |
paramSet_writeToEEProm(1); |
configuration_paramSetDidChange(); |
tempchar[0] = 1; |
beepNumber(tempchar[0]); |
} else { |
tempchar[0] = 0; //indicate bad data |
} |
while (!txd_complete) |
; // wait for previous frame to be sent |
sendOutData('S', FC_ADDRESS, 1, &tempchar, 1); |
} |
break; |
default: |
//unsupported command received |
break; |
} // case FC_ADDRESS: |
default: // any Slave Address |
switch (rxd_buffer[2]) { |
case 'a':// request for labels of the analog debug outputs |
requestedDebugLabel = pRxData[0]; |
if (requestedDebugLabel > 31) |
requestedDebugLabel = 31; |
break; |
case 'b': // submit extern control |
memcpy(&externalControl, (uint8_t*) pRxData, sizeof(externalControl)); |
confirmFrame = externalControl.frame; |
externalControlActive = 255; |
break; |
case 'o':// request for OSD data (FC style) |
OSD_interval = (uint16_t) pRxData[0] * 10; |
if (OSD_interval > 0) |
request_OSD = TRUE; |
break; |
case 'v': // request for version and board release |
request_verInfo = TRUE; |
break; |
case 'x': |
request_variables = TRUE; |
break; |
case 'g':// get external control data |
request_externalControl = TRUE; |
break; |
case 'd': // request for the debug data |
debugData_interval = (uint16_t) pRxData[0] * 10; |
if (debugData_interval > 0) |
request_debugData = TRUE; |
break; |
case 'c': // request for the 3D data |
data3D_interval = (uint16_t) pRxData[0] * 10; |
if (data3D_interval > 0) |
request_data3D = TRUE; |
break; |
default: |
//unsupported command received |
break; |
} |
break; // default: |
} |
// unlock the rxd buffer after processing |
pRxData = 0; |
rxDataLen = 0; |
rxd_buffer_locked = FALSE; |
} |
/************************************************************************/ |
/* Routine f�r die Serielle Ausgabe */ |
/************************************************************************/ |
int16_t uart_putchar(int8_t c) { |
if (c == '\n') |
uart_putchar('\r'); |
// wait until previous character was send |
loop_until_bit_is_set(UCSR0A, UDRE0); |
// send character |
UDR0 = c; |
return (0); |
} |
//--------------------------------------------------------------------------------------------- |
void usart0_transmitTxData(void) { |
if (!txd_complete) |
return; |
if (request_verInfo && txd_complete) { |
sendOutData('V', FC_ADDRESS, 1, (uint8_t *) &versionInfo, sizeof(versionInfo)); |
request_verInfo = FALSE; |
} |
if (requestedDebugLabel != 0xFF && txd_complete) { // Texte f�r die Analogdaten |
uint8_t label[16]; // local sram buffer |
memcpy_P(label, ANALOG_LABEL[requestedDebugLabel], 16); // read lable from flash to sram buffer |
sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &requestedDebugLabel, |
sizeof(requestedDebugLabel), label, 16); |
requestedDebugLabel = 0xFF; |
} |
if (confirmFrame && txd_complete) { // Datensatz ohne checksum best�tigen |
sendOutData('B', FC_ADDRESS, 1, (uint8_t*) &confirmFrame, sizeof(confirmFrame)); |
confirmFrame = 0; |
} |
if (((debugData_interval && checkDelay(debugData_timer)) || request_debugData) |
&& txd_complete) { |
sendOutData('D', FC_ADDRESS, 1, (uint8_t *) &debugOut, sizeof(debugOut)); |
debugData_timer = setDelay(debugData_interval); |
request_debugData = FALSE; |
} |
if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) && txd_complete) { |
sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D)); |
data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg |
data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg |
data3D.heading = (int16_t) (attitude[YAW] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg |
data3D_timer = setDelay(data3D_interval); |
request_data3D = FALSE; |
} |
if (request_externalControl && txd_complete) { |
sendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl, |
sizeof(externalControl)); |
request_externalControl = FALSE; |
} |
if (request_servoTest && txd_complete) { |
sendOutData('T', FC_ADDRESS, 0); |
request_servoTest = FALSE; |
} |
if (request_PPMChannels && txd_complete) { |
sendOutData('P', FC_ADDRESS, 1, (uint8_t *) &PPM_in, sizeof(PPM_in)); |
request_PPMChannels = FALSE; |
} |
if (request_variables && txd_complete) { |
sendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables)); |
request_variables = FALSE; |
} |
if (((OSD_interval && checkDelay(OSD_timer)) || request_OSD) && txd_complete) { |
int32_t height = 0; |
data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg |
data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg |
// TODO: To 0..359 interval. |
data3D.heading = (int16_t) (attitude[YAW] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg |
sendOutData('O', FC_ADDRESS, 3, (uint8_t*)&data3D, sizeof(data3D), (uint8_t*)&height, sizeof(height), (uint8_t*)UBat, sizeof(UBat)); |
OSD_timer = setDelay(OSD_interval); |
request_OSD = FALSE; |
} |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/uart0.h |
---|
0,0 → 1,40 |
#ifndef _UART0_H |
#define _UART0_H |
#define TXD_BUFFER_LEN 180 |
#define RXD_BUFFER_LEN 180 |
#include <inttypes.h> |
//Baud rate of the USART |
#define USART0_BAUD 57600 |
#define DISPLAYBUFFSIZE 80 |
extern int8_t displayBuff[DISPLAYBUFFSIZE]; |
extern uint8_t dispPtr; |
extern void usart0_init(void); |
extern void usart0_transmitTxData(void); |
extern void usart0_processRxData(void); |
extern int16_t uart_putchar(int8_t c); |
// extern uint8_t remotePollDisplayLine; |
extern uint8_t servoTestActive; |
extern uint8_t servoTest[16]; |
typedef struct { |
int16_t anglePitch; // in 0.1 deg |
int16_t angleRoll; // in 0.1 deg |
int16_t heading; // in 0.1 deg |
uint8_t reserved[8]; |
}__attribute__((packed)) Data3D_t; |
typedef struct { |
Data3D_t attitude; |
//GPS_INFO_t GPSInfo; |
int32_t airpressureHeight; |
int16_t batteryVoltage; |
}__attribute__((packed)) OSDData_t; |
#endif //_UART0_H |