• MikroKopter.de
• Wiki
• SHOP
• Datenschutz
• Impressum

FlightCtrlBL-CtrlMK3MagNaviCtrlProjects

Català-Valencià – Catalan中文 – Chinese (Simplified)中文 – Chinese (Traditional)Česky – CzechDansk – DanishNederlands – DutchEnglish – EnglishSuomi – FinnishFrançais – FrenchDeutsch – Germanעברית – Hebrewहिंदी – HindiMagyar – HungarianBahasa Indonesia – IndonesianItaliano – Italian日本語 – Japanese한국어 – KoreanМакедонски – Macedonianमराठी – MarathiNorsk – NorwegianPolski – PolishPortuguês – PortuguesePortuguês – Portuguese (Brazil)Русский – RussianSlovenčina – SlovakSlovenščina – SlovenianEspañol – SpanishSvenska – SwedishTürkçe – TurkishУкраїнська – UkrainianOëzbekcha – Uzbek

Compare Revisions

• This comparison shows the changes necessary to convert path

  → /branches/dongfang_FC_fixedwing @ 2107

  → /branches/dongfang_FC_fixedwing @ 2108

  ↔ Reverse comparison

• Compare Path:	Rev

  With Path:	Rev

Ignore whitespace Rev 2107 → Rev 2108

/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

/branches/dongfang_FC_fixedwing/commands.c
21,7 → 21,7
uint8_t argument = controlMixer_getArgument();
// TODO! Mode change gadget of some kind.
if (!isMotorRunning) {
if (!isFlying) {
if (command == COMMAND_GYROCAL && !repeated) {
// Gyro calinbration, with or without selecting a new parameter-set.
paramSet_readFromEEProm(1);

/branches/dongfang_FC_fixedwing/configuration.c
20,7 → 20,7
VersionInfo_t versionInfo;
FlightMode_t currentFlightMode = FLIGHT_MODE_MANUAL;
volatile uint8_t isMotorRunning = 0;
volatile uint16_t isFlying= 0;
const MMXLATION XLATIONS[] = {
{offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255},

/branches/dongfang_FC_fixedwing/configuration.h
217,7 → 217,7
extern uint8_t boardRelease;
extern uint8_t CPUType;
extern volatile uint8_t isMotorRunning;
extern volatile uint16_t isFlying;
extern FlightMode_t currentFlightMode;
void IMUConfig_default(void);

/branches/dongfang_FC_fixedwing/main.c
132,7 → 132,7
J4LOW;
// Allow Serial Data Transmit if motors must not updated or motors are not running
if (!runFlightControl || !isMotorRunning) {
if (!runFlightControl || !isFlying) {
usart0_transmitTxData();
}

/branches/dongfang_FC_fixedwing/output.c
86,7 → 86,7
*/
void beepNumber(uint8_t numbeeps) {
while(numbeeps--) {
if(isMotorRunning) return; //auf keinen Fall bei laufenden Motoren!
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,

/branches/dongfang_FC_fixedwing/uart0.c
483,7 → 483,7
break;
case 'j':// Save IMU configuration
if (!isMotorRunning) // save settings only if motors are off
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));
520,7 → 520,7
break;
case 's': // save settings
if (!isMotorRunning) // save settings only if motors are off
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
{