Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2131 → Rev 2132

/branches/dongfang_FC_fixedwing/analog.c
433,6 → 433,7
versionInfo.hardwareErrors[0] |= FC_ERROR0_PRESSURE;
 
gyroOffset_writeToEEProm();
airpressureOffset_writeToEEProm();
 
startAnalogConversionCycle();
}
/branches/dongfang_FC_fixedwing/arduino_atmega328/analog.c
41,8 → 41,8
/*
* Airspeed
*/
int16_t airpressure;
uint16_t airspeedVelocity = 0;
//int16_t airpressure;
//uint16_t airspeedVelocity = 0;
//int16_t airpressureWindow[AIRPRESSURE_WINDOW_LENGTH];
//uint8_t airpressureWindowIdx = 0;
 
131,7 → 131,7
 
// ADC channels
#define AD_UBAT 6
#define AD_AIRPRESSURE 7
//#define AD_AIRPRESSURE 7
 
/*
* Table of AD converter inputs for each state.
146,11 → 146,11
*/
 
const uint8_t channelsForStates[] PROGMEM = {
AD_AIRPRESSURE,
AD_UBAT,
AD_AIRPRESSURE,
AD_AIRPRESSURE,
AD_AIRPRESSURE,
//AD_AIRPRESSURE,
AD_UBAT
//AD_AIRPRESSURE,
//AD_AIRPRESSURE,
//AD_AIRPRESSURE,
};
 
// Feature removed. Could be reintroduced later - but should work for all gyro types then.
218,7 → 218,7
}
 
adState = 0;
adChannel = AD_AIRPRESSURE;
adChannel = AD_UBAT;
ADMUX = (ADMUX & 0xE0) | adChannel;
startADC();
sensorDataReady = 0;
300,12 → 300,13
}
}
 
/*
// probably wanna aim at 1/10 m/s/unit.
#define LOG_AIRSPEED_FACTOR 0
 
void analog_updateAirspeed(void) {
uint16_t rawAirpressure = ADSensorInputs[AD_AIRPRESSURE];
int16_t temp = rawAirpressure - airpressureOffset;
int16_t temp = airpressureOffset - rawAirPressure;
// airpressure -= airpressureWindow[airpressureWindowIdx];
// airpressure += temp;
// airpressureWindow[airpressureWindowIdx] = temp;
326,6 → 327,7
isFlying = 0; //(airspeedVelocity >= staticParams.isFlyingThreshold);
}
*/
 
void analog_updateBatteryVoltage(void) {
// Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
336,7 → 338,7
void analog_update(void) {
analog_updateGyros();
// analog_updateAccelerometers();
analog_updateAirspeed();
// analog_updateAirspeed();
analog_updateBatteryVoltage();
#ifdef USE_MK3MAG
magneticHeading = volatileMagneticHeading;
379,7 → 381,7
for (axis = PITCH; axis <= YAW; axis++) {
offsets[axis] += rawGyroValue(axis);
}
offsets[3] += ADSensorInputs[AD_AIRPRESSURE];
// offsets[3] += ADSensorInputs[AD_AIRPRESSURE];
}
for (axis = PITCH; axis <= YAW; axis++) {
386,6 → 388,7
gyroOffset.offsets[axis] = (offsets[axis] + OFFSET_CYCLES / 2) / OFFSET_CYCLES;
}
 
/*
airpressureOffset = (offsets[3] + OFFSET_CYCLES / 2) / OFFSET_CYCLES;
int16_t min = 200;
int16_t max = 1024-200;
392,8 → 395,10
 
if(airpressureOffset < min || airpressureOffset > max)
versionInfo.hardwareErrors[0] |= FC_ERROR0_PRESSURE;
*/
 
gyroOffset_writeToEEProm();
airpressureOffset_writeToEEProm();
 
startAnalogConversionCycle();
}
/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.c
106,14 → 106,14
 
for (uint8_t i=0; i<3; i++) {
staticParams.gyroPID[i].P = 80;
staticParams.gyroPID[i].I = 80;
staticParams.gyroPID[i].I = 40;
staticParams.gyroPID[i].D = 40;
staticParams.gyroPID[i].iMax = 45;
}
 
staticParams.stickIElevator = 60;
staticParams.stickIAilerons = 80;
staticParams.stickIRudder = 25;
staticParams.stickIElevator = 40;
staticParams.stickIAilerons = 60;
staticParams.stickIRudder = 20;
 
// Outputs
staticParams.outputFlash[0].bitmask = 1; //0b01011111;
/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.h
4,8 → 4,7
#include <inttypes.h>
#include <avr/io.h>
 
#define MAX_CHANNELS 10
#define MAX_MOTORS 12
#define MAX_CHANNELS 8
 
// bitmask for VersionInfo_t.HardwareError[0]
#define FC_ERROR0_GYRO_PITCH 0x01
162,8 → 161,6
uint8_t IFactor;
 
uint8_t batteryWarningVoltage;
uint8_t emergencyThrottle;
uint8_t emergencyFlightDuration;
 
// Airspeed
uint8_t airspeedCorrection;
170,14 → 167,14
uint8_t isFlyingThreshold;
 
// Servos
uint8_t controlServosReverse;
uint8_t servoCount;
uint8_t servosReverse;
 
uint8_t servoCount;
uint8_t controlServoMinValue;
uint8_t controlServoMaxValue;
 
uint8_t servoManualMaxSpeed;
Servo_t servoConfigurations[2]; // [PITCH, ROLL]
uint8_t gimbalServoMaxManualSpeed;
Servo_t gimbalServoConfigurations[2]; // [PITCH, ROLL]
 
// Outputs
output_flash_t outputFlash[2];
230,13 → 227,12
 
void IMUConfig_default(void);
void channelMap_default(void);
void rcTrim_setZero(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
8,7 → 8,7
#include "commands.h"
#include "output.h"
 
int16_t controls[4] = { 0, 0, 0, 0 };
int16_t controls[4] = { 0, 0, 0, -1024 };
 
// Internal variables for reading commands made with an R/C stick.
uint8_t lastCommand = COMMAND_NONE;
54,7 → 54,6
int16_t targetvalue;
for (i=0; i < VARIABLE_COUNT; i++) {
targetvalue = RC_getVariable(i);
// if (i<2) debugOut.analog[18+i] = targetvalue;
if (targetvalue < 0)
targetvalue = 0;
if (variables[i] < targetvalue && variables[i] < 255)
108,7 → 107,7
*/
 
void controlMixer_periodicTask(void) {
int16_t tempPRYT[4] = { 0, 0, 0, 0 };
int16_t tempPRYT[4] = { 0, 0, 0, RC_getZeroThrottle()};
 
// Decode commands.
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE;
/branches/dongfang_FC_fixedwing/arduino_atmega328/eeprom.c
144,7 → 144,7
}
 
/***************************************************/
/* ChannelMap */
/* ChannelMap and R/C trim */
/***************************************************/
void channelMap_writeToEEProm(void) {
writeChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(ChannelMap_t));
155,10 → 155,21
printf("\n\rwriting default channel map");
channelMap_default();
channelMap_writeToEEProm();
wdt_enable(WDTO_500MS);
}
}
 
void rcTrim_writeToEEProm(void) {
writeChecksummedBlock(0, (uint8_t*)&rcTrim, EEPROM_ADR_RCTRIM, sizeof(RCTrim_t));
}
 
void rcTrim_readOrDefault(void) {
if (readChecksummedBlock(0, (uint8_t*)&rcTrim, EEPROM_ADR_RCTRIM, sizeof(RCTrim_t))) {
printf("\n\rwriting zero RC trim");
RC_setZeroTrim();
rcTrim_writeToEEProm();
}
}
 
/***************************************************/
/* Sensor offsets */
/***************************************************/
/branches/dongfang_FC_fixedwing/arduino_atmega328/eeprom.h
20,7 → 20,8
#define EEPROM_ADR_GYROOFFSET 32
#define EEPROM_ADR_GYROAMPLIFIER 48
#define EEPROM_ADR_CHANNELMAP 64
#define EEPROM_ADR_IMU_CONFIG 100
#define EEPROM_ADR_RCTRIM 96
#define EEPROM_ADR_IMU_CONFIG 128
#define EEPROM_ADR_PARAMSET_BEGIN 256
 
#define CHANNELMAP_REVISION 1
27,10 → 28,11
#define EEPARAM_REVISION 100
#define EEMIXER_REVISION 0
#define SENSOROFFSET_REVISION 0
#define IMUCONFIG_REVISION 0
#define IMUCONFIG_REVISION 1
 
void paramSet_readOrDefault(void);
void channelMap_readOrDefault(void);
void rcTrim_readOrDefault(void);
void IMUConfig_readOrDefault(void);
void IMUConfig_writeToEEprom(void);
 
/branches/dongfang_FC_fixedwing/arduino_atmega328/flight.c
45,9 → 45,9
 
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;
reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;
reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS;
reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER;
 
// At a switch to angles, we want to kill errors first.
// This should be triggered only once per mode change!
64,19 → 64,11
 
// Normal at airspeed = 10.
uint8_t calcAirspeedPID(uint8_t pid) {
if (staticParams.bitConfig & CFG_USE_AIRSPEED_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);
85,6 → 77,11
}
}
 
#define LOG_STICK_SCALE 8
#define LOG_P_SCALE 6
#define LOG_I_SCALE 10
#define LOG_D_SCALE 6
 
/************************************************************************/
/* Main Flight Control */
/************************************************************************/
107,10 → 104,17
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) >> 7;
target[ROLL] += (controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 7;
target[YAW] += (controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 7;
int32_t tmp;
 
tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE;
if (reverse[PITCH]) target[PITCH] += tmp; else target[PITCH] -= tmp;
 
tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE;
if (reverse[ROLL]) target[ROLL] += tmp; else target[ROLL] -= tmp;
 
tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE;
if (reverse[YAW]) target[YAW] += tmp; else target[YAW] -= tmp;
 
for (axis = PITCH; axis <= YAW; axis++) {
if (target[axis] > OVER180) {
target[axis] -= OVER360;
118,21 → 122,58
target[axis] += OVER360;
}
 
if (reverse[axis])
error[axis] = attitude[axis] + target[axis];
else
error[axis] = attitude[axis] - target[axis];
error[axis] = attitude[axis] - target[axis];
 
#define ROTATETARGET 1
// #define TRUNCATEERROR 1
 
#ifdef ROTATETARGET
if(abs(error[axis]) > OVER180) {
// The shortest way from attitude to target crosses -180.
// Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative.
// Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive.
if (error[axis] > 0) {
if (error[axis] < OVER360 - maxError[axis]) {
// too much err.
error[axis] = -maxError[axis];
target[axis] = attitude[axis] + maxError[axis];
if (target[axis] > OVER180) target[axis] -= OVER360;
} else {
// Normal case, we just need to correct for the wrap. Error will be negative.
error[axis] -= OVER360;
}
} else {
if (error[axis] > maxError[axis] - OVER360) {
// too much err.
error[axis] = maxError[axis];
target[axis] = attitude[axis] - maxError[axis];
if (target[axis] < -OVER180) target[axis] += OVER360;
} else {
// Normal case, we just need to correct for the wrap. Error will be negative.
error[axis] += OVER360;
}
}
} else {
// Simple case, linear range.
if (error[axis] > maxError[axis]) {
error[axis] = maxError[axis];
target[axis] = attitude[axis] - maxError[axis];
} else if (error[axis] < -maxError[axis]) {
error[axis] = -maxError[axis];
target[axis] = attitude[axis] + maxError[axis];
}
}
#endif
#ifdef TUNCATEERROR
if (error[axis] > maxError[axis]) {
error[axis] = maxError[axis];
error[axis] = maxError[axis];
} else if (error[axis] < -maxError[axis]) {
error[axis] =- maxError[axis];
error[axis] = -maxError[axis];
} else {
// update I parts here for angles mode. I parts in rate mode is something different.
}
#endif
 
#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) */
143,25 → 184,20
* (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;
int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;
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];
if (reverse[axis])
term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis];
else
term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis];
}
 
debugOut.analog[12] = term[PITCH];
172,7 → 208,7
for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {
int16_t tmp;
if (servoTestActive) {
controlServos[i] = ((int16_t) servoTest[i] - 128) * 4;
controlServos[i] = ((int16_t) servoTest[i] - 128) * 8;
} else {
// Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
switch (i) {
/branches/dongfang_FC_fixedwing/arduino_atmega328/main.c
1,6 → 1,7
#include <avr/boot.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <util/delay.h>
 
#include "timer0.h"
17,95 → 18,106
#include "eeprom.h"
#include "printf_P.h"
 
uint8_t resetFlag = 0;
 
void reset(void) {
resetFlag = 1;
wdt_enable(WDTO_15MS);
while (1)
;
}
 
int16_t main(void) {
uint16_t timer;
static uint8_t profileTimer;
uint16_t timer = 0;
 
// disable interrupts global
cli();
#ifdef DO_PROFILE
static uint8_t profileTimer;
#endif
 
// disable watchdog
MCUSR &= ~(1 << WDRF);
WDTCSR |= (1 << WDCE) | (1 << WDE);
WDTCSR = 0;
// disable interrupts global
cli();
 
// 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();
wdt_enable(WDTO_120MS);
 
// initalize modules
output_init();
timer0_init();
timer2_init();
usart0_init();
RC_Init();
analog_init();
// initalize modules
output_init();
timer0_init();
timer2_init();
usart0_init();
RC_Init();
analog_init();
 
// Parameter Set handling
IMUConfig_readOrDefault();
channelMap_readOrDefault();
rcTrim_readOrDefault();
paramSet_readOrDefault();
// Parameter Set handling
IMUConfig_readOrDefault();
channelMap_readOrDefault();
rcTrim_readOrDefault();
paramSet_readOrDefault();
 
// enable interrupts global
sei();
// enable interrupts global
sei();
 
controlMixer_setNeutral();
controlMixer_setNeutral();
 
// Cal. attitude sensors and reset integrals.
attitude_setNeutral();
// Cal. attitude sensors and reset integrals.
attitude_setNeutral();
 
// This is not a busy-wait operation and should be OK.
beep(2000);
// This is not a busy-wait operation and should be OK.
beep(2000);
 
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;
}
PD2HIGH;
// 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();
PD2LOW;
while (1) {
if (runFlightControl) { // control interval
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
 
// Allow Serial Data Transmit if motors must not updated or motors are not running
if (!runFlightControl || !isFlying) {
usart0_transmitTxData();
}
if (!resetFlag) {
wdt_reset();
}
 
usart0_processRxData();
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;
}
PD2HIGH;
// 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();
PD2LOW;
 
static uint8_t aboveWarningLimitVoltageSeen = 0;
// Allow Serial Data Transmit if motors must not updated or motors are not running
if (!runFlightControl || !isFlying) {
usart0_transmitTxData();
}
 
if (checkDelay(timer)) {
if (UBat >= staticParams.batteryWarningVoltage) {
aboveWarningLimitVoltageSeen = 1;
} else { // If we are above USB voltage, or if we have once been above warning voltage
if (aboveWarningLimitVoltageSeen || UBat > UBAT_AT_5V) {
beepBatteryAlarm();
}
}
calculateFeaturedServoValues();
timer = setDelay(20); // every 20 ms
}
usart0_processRxData();
 
output_update();
static uint8_t aboveWarningLimitVoltageSeen = 0;
 
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);
if (checkDelay(timer)) {
if (UBat >= staticParams.batteryWarningVoltage) {
aboveWarningLimitVoltageSeen = 1;
} else { // If we are above USB voltage, or if we have once been above warning voltage
if (aboveWarningLimitVoltageSeen || UBat > UBAT_AT_5V) {
beepBatteryAlarm();
}
}
calculateFeaturedServoValues();
timer = setDelay(20); // every 20 ms
}
 
output_update();
 
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/rc.c
10,6 → 10,9
 
// The channel array is 0-based!
volatile int16_t PPM_in[MAX_CHANNELS];
volatile uint16_t RC_buffer[MAX_CHANNELS];
volatile uint8_t inBfrPnt = 0;
 
volatile uint8_t RCQuality;
 
uint8_t lastRCCommand = COMMAND_NONE;
55,6 → 58,21
SREG = sreg;
}
 
/*
* This new and much faster interrupt handler should reduce servo jolts.
*/
ISR(TIMER1_CAPT_vect) {
static uint16_t oldICR1 = 0;
uint16_t signal = (uint16_t)ICR1 - oldICR1;
oldICR1 = ICR1;
//sync gap? (3.5 ms < signal < 25.6 ms)
if (signal > TIME(3.5)) {
inBfrPnt = 0;
} else if (inBfrPnt<MAX_CHANNELS) {
RC_buffer[inBfrPnt++] = signal;
}
}
 
/********************************************************************/
/* Every time a positive edge is detected at PD6 */
/********************************************************************/
76,66 → 94,26
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, 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.5 ms < signal < 25.6 ms)
if (signal > TIME(3.5)) {
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.2 ms)
void RC_process(void) {
if (RCQuality) RCQuality--;
for (uint8_t channel=0; channel<MAX_CHANNELS; channel++) {
uint16_t signal = RC_buffer[channel];
if (signal != 0) {
RC_buffer[channel] = 0; // reset to flag value already used.
if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) {
// shift signal to zero symmetric range -154 to 159
//signal -= 3750; // theoretical value
signal -= (TIME(1.5) - 128 + channelMap.trim-128); // best value with my Futaba in zero trim.
// check for stable signal
if (abs(signal - PPM_in[index]) < TIME(0.05)) {
signal -= (TIME(1.5) - 128 + channelMap.HWTrim);
if (abs(signal - PPM_in[channel]) < TIME(0.05)) {
// With 7 channels and 50 frames/sec, we get 350 channel values/sec.
if (RCQuality < 200)
RCQuality += 10;
else
RCQuality = 200;
RCQuality += 2;
}
// 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
PPM_in[channel] = signal;
}
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_CHANNEL_VERTICAL CH_THROTTLE
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
 
uint8_t getControlModeSwitch(void) {
int16_t channel = RCChannel(CH_MODESWITCH);
160,11 → 138,13
int16_t channel = RCChannel(CH_THROTTLE);
 
if (channel <= -TIME(0.55)) {
lastRCCommand = COMMAND_GYROCAL;
debugOut.analog[17] = 1;
int16_t aux = RCChannel(COMMAND_CHANNEL_HORIZONTAL);
if (abs(aux) >= TIME(0.3)) // If we pull on the stick, it is gyrocal. Else it is RC cal.
lastRCCommand = COMMAND_GYROCAL;
else
lastRCCommand = COMMAND_RCCAL;
} else {
lastRCCommand = COMMAND_NONE;
debugOut.analog[17] = 0;
}
return lastRCCommand;
}
177,13 → 157,12
* Get Pitch, Roll, Throttle, Yaw values
*/
void RC_periodicTaskAndPRYT(int16_t* PRYT) {
if (RCQuality) {
RCQuality--;
PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR);
PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS);
PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER);
PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE);
} // if RCQuality is no good, we just do nothing.
RC_process();
 
PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) - rcTrim.trim[CH_ELEVATOR];
PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) - rcTrim.trim[CH_AILERONS];
PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) - rcTrim.trim[CH_RUDDER];
PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); // no trim on throttle!
}
 
/*
192,12 → 171,12
int16_t RC_getVariable(uint8_t varNum) {
if (varNum < 4)
// 0th variable is 5th channel (1-based) etc.
return (RCChannel(varNum + CH_POTS) >> 2) + channelMap.variableOffset;
return (RCChannel(varNum + CH_POTS) >> 3) + channelMap.variableOffset;
/*
* Let's just say:
* The RC variable i is hardwired to channel i, i>=4
*/
return (PPM_in[varNum] >> 2) + channelMap.variableOffset;
return (PPM_in[varNum] >> 3) + channelMap.variableOffset;
}
 
uint8_t RC_getSignalQuality(void) {
211,9 → 190,18
}
 
void RC_calibrate(void) {
// Do nothing.
rcTrim.trim[CH_ELEVATOR] = RCChannel(CH_ELEVATOR);
rcTrim.trim[CH_AILERONS] = RCChannel(CH_AILERONS);
rcTrim.trim[CH_RUDDER] = RCChannel(CH_RUDDER);
rcTrim.trim[CH_THROTTLE] = 0;
}
 
int16_t RC_getZeroThrottle() {
int16_t RC_getZeroThrottle(void) {
return TIME (-0.5);
}
 
void RC_setZeroTrim(void) {
for (uint8_t i=0; i<MAX_CHANNELS; i++) {
rcTrim.trim[i] = 0;
}
}
/branches/dongfang_FC_fixedwing/arduino_atmega328/rc.h
4,9 → 4,6
#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];
22,6 → 19,10
#define CH_MODESWITCH 4
#define CH_POTS 4
 
// Number of cycles a command must be repeated before commit.
#define COMMAND_TIMER 200
#define COMMAND_CHANNEL_HORIZONTAL CH_AILERONS
 
// void RC_periodicTask(void);
void RC_periodicTaskAndPRYT(int16_t* PRYT);
uint8_t RC_getArgument(void);
29,8 → 30,7
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
int16_t RC_getZeroThrottle(void);
void RC_setZeroTrim(void);
 
#endif //_RC_H
/branches/dongfang_FC_fixedwing/arduino_atmega328/timer0.c
1,6 → 1,7
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include "eeprom.h"
#include "analog.h"
#include "controlMixer.h"
8,8 → 9,10
#include "timer0.h"
#include "output.h"
 
#ifdef USE_MK3MAG
#include "mk3mag.h"
#ifdef DO_PROFILE
volatile uint32_t global10kHzClock = 0;
volatile int32_t profileTimers[NUM_PROFILE_TIMERS];
volatile int32_t runningProfileTimers[NUM_PROFILE_TIMERS];
#endif
 
volatile uint32_t globalMillisClock = 0;
17,10 → 20,6
volatile uint16_t beepTime = 0;
volatile uint16_t beepModulation = BEEP_MODULATION_NONE;
 
#ifdef USE_NAVICTRL
volatile uint8_t SendSPI = 0;
#endif
 
/*****************************************************
* Initialize Timer 0
*****************************************************/
65,6 → 64,12
TIMSK0 &= ~((1 << OCIE0B) | (1 << OCIE0A));
TIMSK0 |= (1 << TOIE0);
 
#ifdef DO_PROFILE
for (uint8_t i=0; i<NUM_PROFILE_TIMERS; i++) {
profileTimers[i] = 0;
}
#endif
 
SREG = sreg;
}
 
75,8 → 80,8
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
#ifdef DO_PROFILE
global10kHzClock++;
#endif
 
if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz)
83,10 → 88,6
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
134,7 → 135,7
void delay_ms(uint16_t w) {
uint16_t t_stop = setDelay(w);
while (!checkDelay(t_stop))
;
wdt_reset();
}
 
// -----------------------------------------------------------------------
142,6 → 143,7
uint16_t t_stop;
t_stop = setDelay(w);
while (!checkDelay(t_stop)) {
wdt_reset();
debugOut.analog[30]++;
if (sensorDataReady == ALL_DATA_READY) {
analog_update();
151,6 → 153,26
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);
while (!sensorDataReady != ALL_DATA_READY) wdt_reset();
}
}
 
#ifdef DO_PROFILE
void startProfileTimer(uint8_t timer) {
runningProfileTimers[timer] = global10kHzClock++;
}
 
void stopProfileTimer(uint8_t timer) {
int32_t t = global10kHzClock++ - runningProfileTimers[timer];
profileTimers[timer] += t;
}
 
void debugProfileTimers(uint8_t index) {
for (uint8_t i=0; i<NUM_PROFILE_TIMERS; i++) {
uint16_t tenths = profileTimers[i] / 1000L;
debugOut.analog[i+index] = tenths;
}
uint16_t tenths = global10kHzClock / 1000L;
debugOut.analog[index + NUM_PROFILE_TIMERS] = tenths;
}
#endif;
/branches/dongfang_FC_fixedwing/arduino_atmega328/timer0.h
23,10 → 23,22
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);
void timer0_init(void);
void delay_ms(uint16_t w);
void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop);
uint16_t setDelay(uint16_t t);
int8_t checkDelay(uint16_t t);
 
#endif //_TIMER0_H
#ifdef DO_PROFILE
#define NUM_PROFILE_TIMERS 4
#define ATTITUDE 0
#define CONTROLMIXER 1
#define COMMANDS 2
#define FLIGHT 3
 
void startProfileTimer(uint8_t timer);
void stopProfileTimer(uint8_t timer);
void debugProfileTimers(uint8_t index);
#endif
 
#endif //_TIMER0_H
/branches/dongfang_FC_fixedwing/arduino_atmega328/timer2.c
81,7 → 81,7
TIMSK2 |= (1 << OCIE2A);
 
for (uint8_t axis=0; axis<2; axis++)
previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR;
previousManualValues[axis] = dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR;
 
SREG = sreg;
}
93,9 → 93,9
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 *= staticParams.gimbalServoConfigurations[axis].stabilizationFactor;
value = value >> 8;
if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE)
if (staticParams.gimbalServoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE)
return -value;
return value;
}
103,7 → 103,7
// With constant-speed limitation.
uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) {
int16_t diff = manualValue - previousManualValues[axis];
uint8_t maxSpeed = staticParams.servoManualMaxSpeed;
uint8_t maxSpeed = staticParams.gimbalServoMaxManualSpeed;
if (diff > maxSpeed) diff = maxSpeed;
else if (diff < -maxSpeed) diff = -maxSpeed;
manualValue = previousManualValues[axis] + diff;
114,11 → 114,11
// 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);
int16_t value = calculateManualServoAxis(axis, dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR);
value += calculateStabilizedServoAxis(axis);
int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR;
int16_t limit = staticParams.gimbalServoConfigurations[axis].minValue * SCALE_FACTOR;
if (value < limit) value = limit;
limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR;
limit = staticParams.gimbalServoConfigurations[axis].maxValue * SCALE_FACTOR;
if (value > limit) value = limit;
value -= (128 * SCALE_FACTOR);
if (value < -SERVOLIMIT) value = -SERVOLIMIT;
131,13 → 131,10
int16_t value;
for (uint8_t axis=0; axis<4; axis++) {
value = controlServos[axis];
value *= 2;
if (value < -SERVOLIMIT) value = -SERVOLIMIT;
else if (value > SERVOLIMIT) value = SERVOLIMIT;
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) {
/branches/dongfang_FC_fixedwing/arduino_atmega328/uart0.c
13,6 → 13,7
#include "output.h"
#include "attitude.h"
#include "commands.h"
#include "main.h"
 
#define FC_ADDRESS 1
#define NC_ADDRESS 2
183,10 → 184,6
// 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;
259,8 → 256,8
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
reset();
}
} else { // checksum invalid
rxd_buffer_locked = FALSE; // unlock rxd buffer
}
/branches/dongfang_FC_fixedwing/configuration.c
16,13 → 16,12
 
uint8_t CPUType;
uint8_t boardRelease;
uint8_t requiredMotors;
 
VersionInfo_t versionInfo;
 
FlightMode_t currentFlightMode = FLIGHT_MODE_MANUAL;
// updated by considering airspeed..
volatile uint16_t isFlying= 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
41,7 → 41,6
uint8_t gyroActivityDamping;
uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung)
uint8_t driftCompLimit; // limit for gyrodrift compensation
 
int8_t gyroCalibrationTweak[3];
} IMUConfig_t;
 
160,7 → 159,7
uint8_t externalControl; // for serial Control
 
uint8_t IFactor;
 
uint8_t batteryWarningVoltage;
 
// Airspeed
168,7 → 167,6
uint8_t isFlyingThreshold;
 
// Servos
 
uint8_t servoCount;
uint8_t servosReverse;
 
/branches/dongfang_FC_fixedwing/eeprom.c
155,8 → 155,6
printf("\n\rwriting default channel map");
channelMap_default();
channelMap_writeToEEProm();
// For whatever stupid reason, the newly defaulted channel map is not used! Reset.
wdt_enable(WDTO_500MS);
}
}
 
/branches/dongfang_FC_fixedwing/flight.c
135,7 → 135,7
#define ROTATETARGET 1
// #define TRUNCATEERROR 1
 
#ifdef(ROTATETARGET)
#ifdef ROTATETARGET
if(abs(error[axis]) > OVER180) {
// The shortest way from attitude to target crosses -180.
// Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative.
145,7 → 145,7
// too much err.
error[axis] = -maxError[axis];
target[axis] = attitude[axis] + maxError[axis];
if (target[axis]) > OVER180) target[axis] -= OVER360;
if (target[axis] > OVER180) target[axis] -= OVER360;
} else {
// Normal case, we just need to correct for the wrap. Error will be negative.
error[axis] -= OVER360;
155,7 → 155,7
// too much err.
error[axis] = maxError[axis];
target[axis] = attitude[axis] - maxError[axis];
if (target[axis]) < -OVER180) target[axis] += OVER360;
if (target[axis] < -OVER180) target[axis] += OVER360;
} else {
// Normal case, we just need to correct for the wrap. Error will be negative.
error[axis] += OVER360;
172,7 → 172,7
}
}
#endif
#ifdef(TUNCATEERROR)
#ifdef TUNCATEERROR
if (error[axis] > maxError[axis]) {
error[axis] = maxError[axis];
} else if (error[axis] < -maxError[axis]) {
182,20 → 182,6
}
#endif
 
/*
* This is the beginning of a version that adjusts the target to differ from the attitude
* by a limited amount. This will eliminate memory over large errors but also knock target angles.
* Idea: The limit could be calculated from the max. servo deflection divided by I factor...
*
*/
/*
if(abs(attitude[axis]-target[axis]) > OVER180) {
if(target[axis] > attitude[axis]) {
 
}
}
*/
 
/************************************************************************/
/* Calculate control feedback from angle (gyro integral) */
/* and angular velocity (gyro signal) */
211,10 → 197,7
 
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.
/branches/dongfang_FC_fixedwing/main.c
1,6 → 1,7
#include <avr/boot.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <util/delay.h>
 
#include "timer0.h"
17,130 → 18,139
#include "eeprom.h"
#include "printf_P.h"
 
uint8_t resetFlag = 0;
 
void reset(void) {
resetFlag = 1;
wdt_enable(WDTO_15MS);
while (1)
;
}
 
int16_t main(void) {
uint16_t timer;
static uint8_t profileTimer;
uint16_t timer = 0;
 
// disable interrupts global
cli();
#ifdef DO_PROFILE
static uint8_t profileTimer;
#endif
 
// analyze hardware environment
setCPUType();
setBoardRelease();
// disable interrupts global
cli();
 
// disable watchdog
MCUSR &= ~(1 << WDRF);
WDTCSR |= (1 << WDCE) | (1 << WDE);
WDTCSR = 0;
wdt_enable(WDTO_120MS);
 
// 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();
// analyze hardware environment
setCPUType();
setBoardRelease();
 
// initalize modules
output_init();
timer0_init();
timer2_init();
usart0_init();
//if (CPUType == ATMEGA644P);// usart1_Init();
RC_Init();
analog_init();
// initalize modules
output_init();
timer0_init();
timer2_init();
usart0_init();
//if (CPUType == ATMEGA644P);// usart1_Init();
RC_Init();
analog_init();
 
// Parameter Set handling
IMUConfig_readOrDefault();
channelMap_readOrDefault();
rcTrim_readOrDefault();
paramSet_readOrDefault();
// Parameter Set handling
IMUConfig_readOrDefault();
channelMap_readOrDefault();
rcTrim_readOrDefault();
paramSet_readOrDefault();
 
// enable interrupts global
sei();
// enable interrupts global
sei();
 
controlMixer_setNeutral();
controlMixer_setNeutral();
 
// Cal. attitude sensors and reset integrals.
attitude_setNeutral();
// Cal. attitude sensors and reset integrals.
attitude_setNeutral();
 
// Init flight parameters
// flight_setNeutral();
// Init flight parameters
// flight_setNeutral();
 
// This is not a busy-wait operation and should be OK.
beep(2000);
// This is not a busy-wait operation and should be OK.
beep(2000);
 
while (1) {
if (runFlightControl) { // control interval
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
while (1) {
if (runFlightControl) { // control interval
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
 
if (!analogDataReady) {
// Analog data should have been ready but is not!!
debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
} else {
debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
}
// 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.
if (!resetFlag) {
wdt_reset();
}
 
if (!analogDataReady) {
// Analog data should have been ready but is not!!
debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
} else {
debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER;
}
// 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.
#ifdef DO_PROFILE
startProfileTimer(ATTITUDE);
startProfileTimer(ATTITUDE);
#endif
calculateFlightAttitude();
calculateFlightAttitude();
#ifdef DO_PROFILE
stopProfileTimer(ATTITUDE);
stopProfileTimer(ATTITUDE);
#endif
 
#ifdef DO_PROFILE
startProfileTimer(CONTROLMIXER);
startProfileTimer(CONTROLMIXER);
#endif
controlMixer_periodicTask();
controlMixer_periodicTask();
#ifdef DO_PROFILE
stopProfileTimer(CONTROLMIXER);
stopProfileTimer(CONTROLMIXER);
#endif
 
#ifdef DO_PROFILE
startProfileTimer(COMMANDS);
startProfileTimer(COMMANDS);
#endif
commands_handleCommands();
commands_handleCommands();
#ifdef DO_PROFILE
stopProfileTimer(COMMANDS);
stopProfileTimer(COMMANDS);
#endif
 
#ifdef DO_PROFILE
startProfileTimer(FLIGHT);
startProfileTimer(FLIGHT);
#endif
flight_control();
flight_control();
#ifdef DO_PROFILE
stopProfileTimer(FLIGHT);
stopProfileTimer(FLIGHT);
#endif
 
// Allow Serial Data Transmit if motors must not updated or motors are not running
if (!runFlightControl || !isFlying) {
usart0_transmitTxData();
}
// Allow Serial Data Transmit if motors must not updated or motors are not running
if (!runFlightControl || !isFlying) {
usart0_transmitTxData();
}
 
usart0_processRxData();
usart0_processRxData();
 
static uint8_t aboveWarningLimitVoltageSeen = 0;
static uint8_t aboveWarningLimitVoltageSeen = 0;
 
if (checkDelay(timer)) {
if (UBat >= staticParams.batteryWarningVoltage) {
aboveWarningLimitVoltageSeen = 1;
} else { // If we are above USB voltage, or if we have once been above warning voltage
if (aboveWarningLimitVoltageSeen || UBat > UBAT_AT_5V) {
beepBatteryAlarm();
}
}
calculateFeaturedServoValues();
timer = setDelay(20); // every 20 ms
}
if (checkDelay(timer)) {
if (UBat >= staticParams.batteryWarningVoltage) {
aboveWarningLimitVoltageSeen = 1;
} else { // If we are above USB voltage, or if we have once been above warning voltage
if (aboveWarningLimitVoltageSeen || UBat > UBAT_AT_5V) {
beepBatteryAlarm();
}
}
calculateFeaturedServoValues();
timer = setDelay(20); // every 20 ms
}
 
output_update();
output_update();
 
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);
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/rc.h
33,8 → 33,4
int16_t RC_getZeroThrottle(void);
void RC_setZeroTrim(void);
 
 
#ifdef USE_MK3MAG
uint8_t RC_testCompassCalState(void);
#endif
#endif //_RC_H
/branches/dongfang_FC_fixedwing/timer0.c
1,6 → 1,7
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include "eeprom.h"
#include "analog.h"
#include "controlMixer.h"
149,7 → 150,7
void delay_ms(uint16_t w) {
uint16_t t_stop = setDelay(w);
while (!checkDelay(t_stop))
;
wdt_reset();
}
 
// -----------------------------------------------------------------------
157,6 → 158,7
uint16_t t_stop;
t_stop = setDelay(w);
while (!checkDelay(t_stop)) {
wdt_reset();
if (analogDataReady) {
analog_update();
startAnalogConversionCycle();
165,7 → 167,7
if (stop) {
// Wait for new samples to get prepared but do not restart AD conversion after that!
// Caller MUST to that.
while (!analogDataReady);
if (!analogDataReady) wdt_reset();
}
}
 
/branches/dongfang_FC_fixedwing/uart0.c
13,6 → 13,7
#include "output.h"
#include "attitude.h"
#include "commands.h"
#include "main.h"
 
#define FC_ADDRESS 1
#define NC_ADDRESS 2
183,10 → 184,6
// 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;
259,8 → 256,8
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
reset();
}
} else { // checksum invalid
rxd_buffer_locked = FALSE; // unlock rxd buffer
}