Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1884 → Rev 1887

/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