Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2123 → Rev 2124

/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 "
};