/branches/dongfang_FC_fixedwing/analog.c |
---|
412,7 → 412,7 |
// determine gyro bias by averaging (requires that the aircraft does not rotate around any axis!) |
for (i = 0; i < OFFSET_CYCLES; i++) { |
delay_ms_with_adc_measurement(2, 1); |
delay_ms_with_adc_measurement(4, 1); |
for (axis = PITCH; axis <= YAW; axis++) { |
offsets[axis] += rawGyroValue(axis); |
} |
/branches/dongfang_FC_fixedwing/commands.c |
---|
7,32 → 7,24 |
#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(); |
if (!isFlying) { |
if (command == COMMAND_GYROCAL && !repeated) { |
if (command == COMMAND_RCCAL && !repeated) { |
RC_calibrate(); |
rcTrim_writeToEEProm(); |
beepNumber(2); // 2 beeps means RC. |
} else if (command == COMMAND_GYROCAL && !repeated) { |
// Gyro calinbration, with or without selecting a new parameter-set. |
paramSet_readFromEEProm(1); |
// paramSet_readFromEEProm(1); |
analog_calibrate(); |
attitude_setNeutral(); |
controlMixer_setNeutral(); |
RC_calibrate(); |
rcTrim_writeToEEProm(); |
beepNumber(1); |
beepNumber(1); // 1 beep means gyro. |
} else if (command == COMMAND_CHMOD && !repeated) { |
configuration_setFlightParameters(argument); |
} |
} // end !MOTOR_RUN condition. |
} |
} |
/branches/dongfang_FC_fixedwing/commands.h |
---|
5,7 → 5,8 |
#define COMMAND_NONE 0 |
#define COMMAND_GYROCAL 1 |
#define COMMAND_CHMOD 2 |
#define COMMAND_RCCAL 2 |
#define COMMAND_CHMOD 3 |
void commands_handleCommands(void); |
/branches/dongfang_FC_fixedwing/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 |
/branches/dongfang_FC_fixedwing/main.c |
---|
121,16 → 121,41 |
} 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; |
#ifdef DO_PROFILE |
startProfileTimer(ATTITUDE); |
#endif |
calculateFlightAttitude(); |
#ifdef DO_PROFILE |
stopProfileTimer(ATTITUDE); |
#endif |
#ifdef DO_PROFILE |
startProfileTimer(CONTROLMIXER); |
#endif |
controlMixer_periodicTask(); |
#ifdef DO_PROFILE |
stopProfileTimer(CONTROLMIXER); |
#endif |
#ifdef DO_PROFILE |
startProfileTimer(COMMANDS); |
#endif |
commands_handleCommands(); |
#ifdef DO_PROFILE |
stopProfileTimer(COMMANDS); |
#endif |
#ifdef DO_PROFILE |
startProfileTimer(FLIGHT); |
#endif |
flight_control(); |
#ifdef DO_PROFILE |
stopProfileTimer(FLIGHT); |
#endif |
// Allow Serial Data Transmit if motors must not updated or motors are not running |
if (!runFlightControl || !isFlying) { |
160,7 → 185,15 |
debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |
} else { |
debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
} |
} |
#ifdef DO_PROFILE |
static uint8_t profileTimer; |
if (profileTimer++ == 0) { |
debugProfileTimers(24); |
} |
#endif |
} |
return (1); |
} |
/branches/dongfang_FC_fixedwing/makefile |
---|
25,6 → 25,10 |
GYRO_CORRECTION=1.0f |
#------------------------------------------------------------------- |
DO_PROFILE=yes |
#------------------------------------------------------------------- |
# get SVN revision |
REV=0001 |
#$(shell sh -c "cat .svn/entries | sed -n '4p'") |
84,6 → 88,7 |
-funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums \ |
-Wall -Wstrict-prototypes \ |
-DGYRO=$(GYRO) \ |
-DDO_PROFILE=$(DO_PROFILE) \ |
-Wa,-adhlns=$(<:.c=.lst) \ |
$(patsubst %,-I%,$(EXTRAINCDIRS)) |
/branches/dongfang_FC_fixedwing/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; |
75,6 → 78,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 */ |
/********************************************************************/ |
96,66 → 114,27 |
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.HWTrim); |
// check for stable signal |
if (abs(signal - PPM_in[index]) < TIME(0.05)) { |
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; |
// } |
} |
} |
debugOut.analog[31] = RCQuality; |
} |
#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); |
180,7 → 159,11 |
int16_t channel = RCChannel(CH_THROTTLE); |
if (channel <= -TIME(0.55)) { |
lastRCCommand = COMMAND_GYROCAL; |
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; |
} |
195,19 → 178,17 |
* Get Pitch, Roll, Throttle, Yaw values |
*/ |
void RC_periodicTaskAndPRYT(int16_t* PRYT) { |
if (RCQuality) { |
RCQuality--; |
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! |
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! |
debugOut.analog[20] = PRYT[CONTROL_ELEVATOR]; |
debugOut.analog[21] = PRYT[CONTROL_AILERONS]; |
debugOut.analog[22] = PRYT[CONTROL_RUDDER]; |
debugOut.analog[23] = PRYT[CONTROL_THROTTLE]; |
} // if RCQuality is no good, we just do nothing. |
debugOut.analog[20] = PRYT[CONTROL_ELEVATOR]; |
debugOut.analog[21] = PRYT[CONTROL_AILERONS]; |
debugOut.analog[22] = PRYT[CONTROL_RUDDER]; |
debugOut.analog[23] = PRYT[CONTROL_THROTTLE]; |
} |
/* |
/branches/dongfang_FC_fixedwing/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); |
/branches/dongfang_FC_fixedwing/timer0.c |
---|
8,8 → 8,9 |
#include "timer0.h" |
#include "output.h" |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#ifdef DO_PROFILE |
uint32_t profileTimers[NUM_PROFILE_TIMERS]; |
uint32_t runningProfileTimers[NUM_PROFILE_TIMERS]; |
#endif |
volatile uint32_t globalMillisClock = 0; |
17,10 → 18,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 |
*****************************************************/ |
74,6 → 71,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; |
} |
84,18 → 87,10 |
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 |
168,3 → 163,23 |
while (!analogDataReady); |
} |
} |
#ifdef DO_PROFILE |
void startProfileTimer(uint8_t timer) { |
runningProfileTimers[timer] = globalMillisClock; |
} |
void stopProfileTimer(uint8_t timer) { |
int32_t t = globalMillisClock - 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] / 10000L; |
debugOut.analog[i+index] = tenths; |
} |
uint16_t tenths = globalMillisClock / 10000L; |
debugOut.analog[index + NUM_PROFILE_TIMERS] = tenths; |
} |
#endif; |
/branches/dongfang_FC_fixedwing/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); |
#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/uart0.c |
---|
105,13 → 105,13 |
"RC R ", |
"RC Y ", |
"RC T ", |
"Servo P ", |
"Servo R ", //25 |
"Servo T ", |
"Servo Y ", |
"RCPulseCount ", |
"Var0 . ", |
"Var1 ", //30 |
"Profile 0 ", |
"Profile 1 ", //25 |
"Profile 2 ", |
"Profile 3 ", |
"Profile total ", |
" ", |
" ", //30 |
"RCQuality " |
}; |