/branches/dongfang_FC_rewrite/analog.c |
---|
502,7 → 502,7 |
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
for (i = 0; i < GYRO_OFFSET_CYCLES; i++) { |
Delay_ms_Mess(20); |
delay_ms_Mess(20); |
for (axis = PITCH; axis <= YAW; axis++) { |
deltaOffsets[axis] += rawGyroSum[axis]; |
} |
524,7 → 524,7 |
// airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
// pressureMeasurementCount = 0; |
Delay_ms_Mess(100); |
delay_ms_Mess(100); |
} |
/* |
542,7 → 542,7 |
// int16_t pressureDiff, savedRawAirPressure; |
for (i = 0; i < ACC_OFFSET_CYCLES; i++) { |
Delay_ms_Mess(10); |
delay_ms_Mess(10); |
for (axis = PITCH; axis <= YAW; axis++) { |
deltaOffset[axis] += acc[axis]; |
} |
565,13 → 565,13 |
// Setting offset values has an influence in the analog.c ISR |
// Therefore run measurement for 100ms to achive stable readings |
Delay_ms_Mess(100); |
delay_ms_Mess(100); |
// Set the feedback so that air pressure ends up in the middle of the range. |
// (raw pressure high --> OCR0A also high...) |
/* |
OCR0A += ((rawAirPressure - 1024) / rangewidth) - 1; |
Delay_ms_Mess(1000); |
delay_ms_Mess(1000); |
pressureDiff = 0; |
// DebugOut.Analog[16] = rawAirPressure; |
580,12 → 580,12 |
for (i=0; i<PRESSURE_CAL_CYCLE_COUNT; i++) { |
savedRawAirPressure = rawAirPressure; |
OCR0A+=2; |
Delay_ms_Mess(500); |
delay_ms_Mess(500); |
// raw pressure will decrease. |
pressureDiff += (savedRawAirPressure - rawAirPressure); |
savedRawAirPressure = rawAirPressure; |
OCR0A-=2; |
Delay_ms_Mess(500); |
delay_ms_Mess(500); |
// raw pressure will increase. |
pressureDiff += (rawAirPressure - savedRawAirPressure); |
} |
/branches/dongfang_FC_rewrite/attitudeControl.c |
---|
8,8 → 8,8 |
#include "output.h" |
// = cos^2(45 degs). |
const int32_t FACTORSQUARED = (int32_t) (1<<(MATH_UNIT_FACTOR_LOG*2)); |
const int32_t MINPROJECTION = (int32_t) (1<<(MATH_UNIT_FACTOR_LOG*2-1)); |
// const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2); |
const int32_t MINPROJECTION = 1L << (MATH_UNIT_FACTOR_LOG * 2 - 9); |
// Takes 380 - 400 usec. Way too slow. |
// With static MINPROJECTION: 220 usec. |
16,22 → 16,25 |
uint16_t AC_getThrottle(uint16_t throttle) { |
int32_t projection; |
uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3 |
int16_t deltaThrottle; |
projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]); |
/* |
int16_t deltaThrottle, y; |
projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]); |
projection >>= 8; |
if (projection < 0) { |
// Case not yet considered! |
y = 0; |
} else { |
if (projection < MINPROJECTION && projection >= 0) { |
deltaThrottle = 0; |
projection = MINPROJECTION; |
} else if (projection > -MINPROJECTION && projection < 0) { |
// projection = -MINPROJECITON; |
deltaThrottle = 0; |
projection = -MINPROJECTION; |
} else { |
*/ |
deltaThrottle = (((int32_t) effect * throttle) << (MATH_UNIT_FACTOR*2)) / (projection * 100) - (1 << (MATH_UNIT_FACTOR*2)); |
// DebugOut.Analog[13] = deltaThrottle; |
} |
y = ((int32_t) throttle << (MATH_UNIT_FACTOR_LOG * 2 - 8)) / projection |
- throttle; |
} |
deltaThrottle = ((int32_t)y * effect) >> 6; |
DebugOut.Analog[8] = deltaThrottle; |
return throttle + deltaThrottle; |
} |
/branches/dongfang_FC_rewrite/commands.c |
---|
55,6 → 55,7 |
#include "flight.h" |
#include "eeprom.h" |
#include "attitude.h" |
#include "output.h" |
void commands_handleCommands(void) { |
/* |
/branches/dongfang_FC_rewrite/configuration.c |
---|
53,7 → 53,6 |
#include <stddef.h> |
#include "configuration.h" |
#include "eeprom.h" |
#include "timer0.h" |
int16_t variables[8] = {0,0,0,0,0,0,0,0}; |
dynamicParam_t dynamicParams = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0}; |
234,60 → 233,3 |
GRN_OFF; |
return BoardRelease; |
} |
void beep(uint16_t millis) { |
BeepTime = millis; |
} |
/* |
* Make [numbeeps] beeps. |
*/ |
void beepNumber(uint8_t numbeeps) { |
while(numbeeps--) { |
if(MKFlags & MKFLAG_MOTOR_RUN) 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 == 0xFFFF) { // If not already beeping an alarm signal (?) |
BeepTime = 15000; // 1.5 seconds |
BeepModulation = 0x0C00; |
} |
} |
/* |
* Beep the I2C bus error signal |
*/ |
void beepI2CAlarm(void) { |
if((BeepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN)) { |
BeepTime = 10000; // 1 second |
BeepModulation = 0x0080; |
} |
} |
/* |
* Beep the battery low alarm signal |
*/ |
void beepBatteryAlarm(void) { |
BeepModulation = 0x0300; |
if(!BeepTime) { |
BeepTime = 6000; // 0.6 seconds |
} |
} |
/* |
* Beep the EEPROM checksum alarm |
*/ |
void beepEEPROMAlarm(void) { |
BeepModulation = 0x0007; |
if(!BeepTime) { |
BeepTime = 6000; // 0.6 seconds |
} |
} |
/branches/dongfang_FC_rewrite/configuration.h |
---|
172,7 → 172,7 |
#define ATMEGA644P 1 |
#define SYSCLK F_CPU |
// Not really a part of configuration, but heck, LEDs and beepers now have a home. |
// Not really a part of configuration, but LEDs and HW version test are the same. |
#define RED_OFF {if((BoardRelease == 10) || (BoardRelease == 20)) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);} |
#define RED_ON {if((BoardRelease == 10) || (BoardRelease == 20)) PORTB |= (1<<PORTB0); else PORTB &=~(1<<PORTB0);} |
#define RED_FLASH PORTB ^= (1<<PORTB0) |
195,11 → 195,4 |
uint8_t getCPUType(void); |
uint8_t getBoardRelease(void); |
// Not really a part of configuration, but heck, LEDs and beepers now have a home. |
void beep(uint16_t millis); |
void beepNumber(uint8_t numbeeps); |
void beepRCAlarm(void); |
void beepI2CAlarm(void); |
void beepBatteryAlarm(void); |
#endif // _CONFIGURATION_H |
/branches/dongfang_FC_rewrite/invenSense.c |
---|
15,21 → 15,21 |
#define AUTOZERO_DDR DDRD |
#define AUTOZERO_BIT 5 |
void gyro_calibrate() { |
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); |
delay_ms(100); |
} |
// Make a pulse on the auto-zero output line. |
AUTOZERO_PORT &= ~(1 << AUTOZERO_BIT); |
Delay_ms(1); |
delay_ms(1); |
AUTOZERO_PORT |= (1 << AUTOZERO_BIT); |
// Delay_ms(10); |
Delay_ms_Mess(100); |
delay_ms_Mess(100); |
} |
void gyro_setDefaults(void) { |
/branches/dongfang_FC_rewrite/main.c |
---|
136,23 → 136,23 |
// 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); |
timer = setDelay(200); |
OUTPUT_SET(0,1); |
GRN_OFF; |
RED_ON; |
while (!CheckDelay(timer)) |
while (!checkDelay(timer)) |
; |
timer = SetDelay(200); |
timer = setDelay(200); |
OUTPUT_SET(0,0); |
OUTPUT_SET(1,1); |
RED_OFF; |
GRN_ON; |
while (!CheckDelay(timer)) |
while (!checkDelay(timer)) |
; |
timer = SetDelay(200); |
while (!CheckDelay(timer)) |
timer = setDelay(200); |
while (!checkDelay(timer)) |
; |
OUTPUT_SET(1,0); |
186,8 → 186,8 |
#endif |
#if (defined (USE_MK3MAG)) |
if(CPUType == ATMEGA644P) printf("\n\rSupport for GPS at 2nd UART"); |
else printf("\n\rSupport for GPS at 1st UART"); |
if(CPUType == ATMEGA644P) printf("\n\r2 UART"); |
else printf("\n\r1 UART"); |
#endif |
controlMixer_setNeutral(); |
244,7 → 244,7 |
usart0_ProcessRxData(); |
if (CheckDelay(timer)) { |
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. |
256,7 → 256,7 |
SPI_StartTransmitPacket(); |
SendSPI = 4; |
#endif |
timer = SetDelay(20); // every 20 ms |
timer = setDelay(20); // every 20 ms |
} |
output_update(); |
} |
/branches/dongfang_FC_rewrite/makefile |
---|
22,11 → 22,11 |
#RC = DSL |
#RC = SPECTRUM |
GYRO=ENC-03_FC1.3 |
GYRO_HW_NAME=ENC |
GYRO_HW_FACTOR=1.304f |
GYRO_PITCHROLL_CORRECTION=1.0f |
GYRO_YAW_CORRECTION=1.0f |
#GYRO=ENC-03_FC1.3 |
#GYRO_HW_NAME=ENC |
#GYRO_HW_FACTOR=1.304f |
#GYRO_PITCHROLL_CORRECTION=1.0f |
#GYRO_YAW_CORRECTION=1.0f |
#GYRO=ADXRS610_FC2.0 |
#GYRO_HW_NAME=ADXR |
34,11 → 34,11 |
#GYRO_PITCHROLL_CORRECTION=1.0f |
#GYRO_YAW_CORRECTION=1.0f |
#GYRO=invenSense |
#GYRO_HW_NAME=Isense |
#GYRO_HW_FACTOR=0.6827f |
#GYRO_PITCHROLL_CORRECTION=1.0f |
#GYRO_YAW_CORRECTION=1.0f |
GYRO=invenSense |
GYRO_HW_NAME=Isense |
GYRO_HW_FACTOR=0.6827f |
GYRO_PITCHROLL_CORRECTION=1.0f |
GYRO_YAW_CORRECTION=1.0f |
#------------------------------------------------------------------- |
# get SVN revision |
/branches/dongfang_FC_rewrite/mk3mag.c |
---|
81,7 → 81,7 |
/*********************************************/ |
void MK3MAG_Update(void) {// called every 102.4 us by timer 0 ISR |
static uint16_t PWMCount = 0; |
static uint16_t BeepDelay = 0; |
static uint16_t beepDelay = 0; |
static uint16_t debugCounter = 0; |
// The pulse width varies from 1ms (0°) to 36.99ms (359.9°) |
// in other words 100us/° with a +1ms offset. |
129,10 → 129,10 |
PWMCount = 0; // reset pwm counter |
} |
if (!PWMTimeout) { |
if (CheckDelay(BeepDelay)) { |
if (!BeepTime) |
BeepTime = 100; // make noise with 10Hz to signal the compass problem |
BeepDelay = SetDelay(100); |
if (checkDelay(beepDelay)) { |
if (!beepTime) |
beepTime = 100; // make noise with 10Hz to signal the compass problem |
beepDelay = setDelay(100); |
} |
} |
} |
/branches/dongfang_FC_rewrite/output.c |
---|
51,6 → 51,7 |
#include <inttypes.h> |
#include "output.h" |
#include "eeprom.h" |
#include "timer0.h" |
// To access the DebugOut struct. |
#include "uart0.h" |
115,3 → 116,60 |
OUTPUT_SET(1, DebugOut.Digital[1] & DIGITAL_DEBUG_MASK); |
} |
} |
void beep(uint16_t millis) { |
beepTime = millis; |
} |
/* |
* Make [numbeeps] beeps. |
*/ |
void beepNumber(uint8_t numbeeps) { |
while(numbeeps--) { |
if(MKFlags & MKFLAG_MOTOR_RUN) 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 == 0xFFFF) { // If not already beeping an alarm signal (?) |
beepTime = 15000; // 1.5 seconds |
beepModulation = 0x0C00; |
} |
} |
/* |
* Beep the I2C bus error signal |
*/ |
void beepI2CAlarm(void) { |
if((beepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN)) { |
beepTime = 10000; // 1 second |
beepModulation = 0x0080; |
} |
} |
/* |
* Beep the battery low alarm signal |
*/ |
void beepBatteryAlarm(void) { |
beepModulation = 0x0300; |
if(!beepTime) { |
beepTime = 6000; // 0.6 seconds |
} |
} |
/* |
* Beep the EEPROM checksum alarm |
*/ |
void beepEEPROMAlarm(void) { |
beepModulation = 0x0007; |
if(!beepTime) { |
beepTime = 6000; // 0.6 seconds |
} |
} |
/branches/dongfang_FC_rewrite/output.h |
---|
16,7 → 16,7 |
#define J5TOGGLE PORTD ^= (1<<PORTD3) |
// invert means: An "1" bit in digital debug data make a LOW on the output. |
#define DIGITAL_DEBUG_INVERT 1 |
#define DIGITAL_DEBUG_INVERT 0 |
#define OUTPUT_HIGH(num) {PORTC |= (4 << (num));} |
#define OUTPUT_LOW(num) {PORTC &= ~(4 << (num));} |
63,9 → 63,14 |
* 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 DEBUG_MAINLOOP_TIMER |
#define DIGITAL_DEBUG_MASK DEBUG_CLIP |
void output_init(void); |
void output_update(void); |
void beep(uint16_t millis); |
void beepNumber(uint8_t numbeeps); |
void beepRCAlarm(void); |
void beepI2CAlarm(void); |
void beepBatteryAlarm(void); |
#endif //_output_H |
/branches/dongfang_FC_rewrite/timer0.c |
---|
62,11 → 62,11 |
#include "mk3mag.h" |
#endif |
volatile uint16_t CountMilliseconds = 0; |
volatile uint8_t runFlightControl = 0; |
volatile uint16_t millisecondsCount = 0; |
volatile uint8_t runFlightControl = 0; |
volatile uint16_t cntKompass = 0; |
volatile uint16_t BeepTime = 0; |
volatile uint16_t BeepModulation = 0xFFFF; |
volatile uint16_t beepTime = 0; |
volatile uint16_t beepModulation = 0xFFFF; |
#ifdef USE_NAVICTRL |
volatile uint8_t SendSPI = 0; |
136,7 → 136,7 |
ISR(TIMER0_OVF_vect) |
{ // 9765.625 Hz |
static uint8_t cnt_1ms = 1, cnt = 0; |
uint8_t Beeper_On = 0; |
uint8_t beeper_On = 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 |
152,23 → 152,23 |
DebugOut.Digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz) |
} |
CountMilliseconds++; // increment millisecond counter |
millisecondsCount++; // increment millisecond counter |
} |
// beeper on if duration is not over |
if (BeepTime) { |
BeepTime--; // decrement BeepTime |
if (BeepTime & BeepModulation) |
Beeper_On = 1; |
if (beepTime) { |
beepTime--; // decrement BeepTime |
if (beepTime & beepModulation) |
beeper_On = 1; |
else |
Beeper_On = 0; |
beeper_On = 0; |
} else { // beeper off if duration is over |
Beeper_On = 0; |
BeepModulation = 0xFFFF; |
beeper_On = 0; |
beepModulation = 0xFFFF; |
} |
// if beeper is on |
if (Beeper_On) { |
if (beeper_On) { |
// set speaker port to high. |
if (BoardRelease == 10) |
PORTD |= (1 << PORTD2); // Speaker at PD2 |
193,27 → 193,27 |
} |
// ----------------------------------------------------------------------- |
uint16_t SetDelay(uint16_t t) { |
return (CountMilliseconds + t - 1); |
uint16_t setDelay(uint16_t t) { |
return (millisecondsCount + t - 1); |
} |
// ----------------------------------------------------------------------- |
int8_t CheckDelay(uint16_t t) { |
return (((t - CountMilliseconds) & 0x8000) >> 8); // check sign bit |
int8_t checkDelay(uint16_t t) { |
return (((t - millisecondsCount) & 0x8000) >> 8); // check sign bit |
} |
// ----------------------------------------------------------------------- |
void Delay_ms(uint16_t w) { |
uint16_t t_stop = SetDelay(w); |
while (!CheckDelay(t_stop)) |
void delay_ms(uint16_t w) { |
uint16_t t_stop = setDelay(w); |
while (!checkDelay(t_stop)) |
; |
} |
// ----------------------------------------------------------------------- |
void Delay_ms_Mess(uint16_t w) { |
void delay_ms_Mess(uint16_t w) { |
uint16_t t_stop; |
t_stop = SetDelay(w); |
while (!CheckDelay(t_stop)) { |
t_stop = setDelay(w); |
while (!checkDelay(t_stop)) { |
if (analogDataReady) { |
analogDataReady = 0; |
analog_start(); |
/branches/dongfang_FC_rewrite/timer0.h |
---|
3,19 → 3,19 |
#include <inttypes.h> |
extern volatile uint16_t CountMilliseconds; |
extern volatile uint8_t runFlightControl; |
extern volatile uint16_t millisecondsCount; |
extern volatile uint8_t runFlightControl; |
extern volatile uint16_t cntKompass; |
extern volatile uint16_t BeepModulation; |
extern volatile uint16_t BeepTime; |
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_Mess(uint16_t w); |
extern uint16_t SetDelay(uint16_t t); |
extern int8_t CheckDelay(uint16_t t); |
extern void delay_ms(uint16_t w); |
extern void delay_ms_Mess(uint16_t w); |
extern uint16_t setDelay(uint16_t t); |
extern int8_t checkDelay(uint16_t t); |
#endif //_TIMER0_H |
/branches/dongfang_FC_rewrite/uart0.c |
---|
131,7 → 131,7 |
"GyroYaw ", //5 |
"GYRO_RATE_FACTOR", |
"GYRO_RATE_FACTOR", |
"unused ", |
"AttitudeControl ", |
"AccPitch (angle)", |
"AccRoll (angle) ", //10 |
"UBat ", |
214,7 → 214,7 |
UCSR0B |= (1 << TXCIE0); |
// initialize the debug timer |
DebugData_Timer = SetDelay(DebugData_Interval); |
DebugData_Timer = setDelay(DebugData_Interval); |
// unlock rxd_buffer |
rxd_buffer_locked = FALSE; |
225,7 → 225,7 |
txd_complete = TRUE; |
#ifdef USE_MK3MAG |
Compass_Timer = SetDelay(220); |
Compass_Timer = setDelay(220); |
#endif |
UART_VersionInfo.SWMajor = VERSION_MAJOR; |
705,14 → 705,14 |
ConfirmFrame = 0; |
} |
if (((DebugData_Interval && CheckDelay(DebugData_Timer)) || request_DebugData) |
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); |
DebugData_Timer = setDelay(DebugData_Interval); |
request_DebugData = FALSE; |
} |
if (((Data3D_Interval && CheckDelay(Data3D_Timer)) || request_Data3D) |
if (((Data3D_Interval && checkDelay(Data3D_Timer)) || request_Data3D) |
&& txd_complete) { |
SendOutData('C', FC_ADDRESS, 1, (uint8_t *) &Data3D, sizeof(Data3D)); |
Data3D.AngleNick = (int16_t) ((10 * angle[PITCH]) |
720,7 → 720,7 |
Data3D.AngleRoll = (int16_t) ((10 * angle[ROLL]) |
/ GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
Data3D.Heading = (int16_t) ((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
Data3D_Timer = SetDelay(Data3D_Interval); |
Data3D_Timer = setDelay(Data3D_Interval); |
request_Data3D = FALSE; |
} |
731,7 → 731,7 |
} |
#ifdef USE_MK3MAG |
if((CheckDelay(Compass_Timer)) && txd_complete) { |
if((checkDelay(Compass_Timer)) && txd_complete) { |
ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0]; |
740,7 → 740,7 |
SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag)); |
// the last state is 5 and should be send only once to avoid multiple flash writing |
if(compassCalState > 4) compassCalState = 0; |
Compass_Timer = SetDelay(99); |
Compass_Timer = setDelay(99); |
} |
#endif |