77,229 → 77,234 |
#endif |
#include "eeprom.h" |
|
int16_t main (void) { |
uint16_t timer; |
int16_t main(void) { |
uint16_t timer; |
|
// disable interrupts global |
cli(); |
// disable interrupts global |
cli(); |
|
// analyze hardware environment |
CPUType = getCPUType(); |
BoardRelease = getBoardRelease(); |
// analyze hardware environment |
CPUType = getCPUType(); |
BoardRelease = getBoardRelease(); |
|
// disable watchdog |
MCUSR &=~(1<<WDRF); |
WDTCSR |= (1<<WDCE)|(1<<WDE); |
WDTCSR = 0; |
// disable watchdog |
MCUSR &= ~(1 << WDRF); |
WDTCSR |= (1 << WDCE) | (1 << WDE); |
WDTCSR = 0; |
|
// PPM_in[CH_THROTTLE] = 0; |
// Why??? They are already initialized to 0. |
// stickPitch = stickRoll = stickYaw = 0; |
// PPM_in[CH_THROTTLE] = 0; |
// Why??? They are already initialized to 0. |
// stickPitch = stickRoll = stickYaw = 0; |
|
RED_OFF; |
RED_OFF; |
|
// initalize modules |
output_init(); |
timer0_init(); |
timer2_init(); |
usart0_Init(); |
if(CPUType == ATMEGA644P) usart1_Init(); |
RC_Init(); |
analog_init(); |
I2C_init(); |
// initalize modules |
output_init(); |
timer0_init(); |
timer2_init(); |
usart0_Init(); |
if (CPUType == ATMEGA644P) |
usart1_Init(); |
RC_Init(); |
analog_init(); |
I2C_init(); |
#ifdef USE_NAVICTRL |
SPI_MasterInit(); |
SPI_MasterInit(); |
#endif |
#ifdef USE_MK3MAG |
MK3MAG_Init(); |
MK3MAG_Init(); |
#endif |
|
// enable interrupts global |
sei(); |
|
printf("\n\r==================================="); |
printf("\n\rFlightControl"); |
printf("\n\rHardware: Custom"); |
printf("\r\n CPU: Atmega644"); |
if(CPUType == ATMEGA644P) |
printf("p"); |
printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
printf("\n\r==================================="); |
// enable interrupts global |
sei(); |
|
// Parameter Set handling |
ParamSet_Init(); |
printf("\n\r==================================="); |
printf("\n\rFlightControl"); |
printf("\n\rHardware: Custom"); |
printf("\r\n CPU: Atmega644"); |
if (CPUType == ATMEGA644P) |
printf("p"); |
printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
printf("\n\r==================================="); |
|
// Wait for a short time (otherwise the RC channel check won't work below) |
// timer = SetDelay(500); |
// while(!CheckDelay(timer)); |
// Parameter Set handling |
ParamSet_Init(); |
|
// 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); |
OUTPUT_SET(0,1); |
GRN_OFF; |
RED_ON; |
while(!CheckDelay(timer)); |
// Wait for a short time (otherwise the RC channel check won't work below) |
// timer = SetDelay(500); |
// while(!CheckDelay(timer)); |
|
timer = SetDelay(200); |
OUTPUT_SET(0,0); |
OUTPUT_SET(1,1); |
RED_OFF; |
GRN_ON; |
while(!CheckDelay(timer)); |
// 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); |
OUTPUT_SET(0,1); |
GRN_OFF; |
RED_ON; |
while (!CheckDelay(timer)) |
; |
|
timer = SetDelay(200); |
while(!CheckDelay(timer)); |
OUTPUT_SET(1,0); |
timer = SetDelay(200); |
OUTPUT_SET(0,0); |
OUTPUT_SET(1,1); |
RED_OFF; |
GRN_ON; |
while (!CheckDelay(timer)) |
; |
|
twi_diagnostics(); |
timer = SetDelay(200); |
while (!CheckDelay(timer)) |
; |
OUTPUT_SET(1,0); |
|
printf("\n\r==================================="); |
twi_diagnostics(); |
|
/* |
if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) |
{ |
printf("\n\rCalibrating air pressure sensor.."); |
timer = SetDelay(1000); |
SearchAirPressureOffset(); |
while (!CheckDelay(timer)); |
printf("OK\n\r"); |
} |
*/ |
printf("\n\r==================================="); |
|
/* |
if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) |
{ |
printf("\n\rCalibrating air pressure sensor.."); |
timer = SetDelay(1000); |
SearchAirPressureOffset(); |
while (!CheckDelay(timer)); |
printf("OK\n\r"); |
} |
*/ |
|
#ifdef USE_NAVICTRL |
printf("\n\rSupport for NaviCtrl"); |
printf("\n\rSupport for NaviCtrl"); |
#ifdef USE_RC_DSL |
printf("\r\nSupport for DSL RC at 2nd UART"); |
printf("\r\nSupport for DSL RC at 2nd UART"); |
#endif |
#ifdef USE_RC_SPECTRUM |
printf("\r\nSupport for SPECTRUM RC at 2nd UART"); |
printf("\r\nSupport for SPECTRUM RC at 2nd UART"); |
#endif |
#endif |
|
#ifdef USE_MK3MAG |
printf("\n\rSupport for MK3MAG Compass"); |
printf("\n\rSupport for MK3MAG Compass"); |
#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\rSupport for GPS at 2nd UART"); |
else printf("\n\rSupport for GPS at 1st UART"); |
#endif |
|
controlMixer_setNeutral(); |
controlMixer_setNeutral(); |
|
// Cal. attitude sensors and reset integrals. |
attitude_setNeutral(); |
// Cal. attitude sensors and reset integrals. |
attitude_setNeutral(); |
|
Servo_On(); |
Servo_On(); |
|
// Init flight parameters |
flight_setNeutral(); |
// Init flight parameters |
flight_setNeutral(); |
|
// RED_OFF; |
// RED_OFF; |
|
beep(2000); |
|
printf("\n\rControl: "); |
if (staticParams.GlobalConfig & CFG_HEADING_HOLD) printf("HeadingHold"); |
else printf("Neutral (ACC-Mode)"); |
beep(2000); |
|
printf("\n\n\r"); |
printf("\n\rControl: "); |
if (staticParams.GlobalConfig & CFG_HEADING_HOLD) |
printf("HeadingHold"); |
else printf("Neutral (ACC-Mode)"); |
|
LCD_Clear(); |
printf("\n\n\r"); |
|
I2CTimeout = 5000; |
LCD_Clear(); |
|
while (1) { |
if(runFlightControl && analogDataReady) { // control interval |
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
|
J4HIGH; |
flight_control(); |
J4LOW; |
|
/* |
* If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit |
* the throttle vector just computed. Otherwise, if motor test is engaged, transmit |
* the test throttle vector. If no testing, stop all motors. |
*/ |
// Obsoleted. |
// transmitMotorThrottleData(); |
|
RED_OFF; |
|
/* |
Does not belong here. Instead, external control should be ignored in |
controlMixer if there was no new data from there for some time. |
if(externalControlActive) externalControlActive--; |
else { |
externalControl.config = 0; |
externalStickPitch = 0; |
externalStickRoll = 0; |
externalStickYaw = 0; |
} |
*/ |
|
/* |
Does not belong here. |
if(RC_Quality) RC_Quality--; |
*/ |
|
/* Does not belong here. Well since we are not supporting navi right now anyway, leave out. |
I2CTimeout = 5000; |
|
while (1) { |
if (runFlightControl && analogDataReady) { // control interval |
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
|
J4HIGH; |
flight_control(); |
J4LOW; |
|
/* |
* If the motors are running (MKFlags & MKFLAG_MOTOR_RUN in flight.c), transmit |
* the throttle vector just computed. Otherwise, if motor test is engaged, transmit |
* the test throttle vector. If no testing, stop all motors. |
*/ |
// Obsoleted. |
// transmitMotorThrottleData(); |
|
RED_OFF; |
|
/* |
Does not belong here. Instead, external control should be ignored in |
controlMixer if there was no new data from there for some time. |
if(externalControlActive) externalControlActive--; |
else { |
externalControl.config = 0; |
externalStickPitch = 0; |
externalStickRoll = 0; |
externalStickYaw = 0; |
} |
*/ |
|
/* |
Does not belong here. |
if(RC_Quality) RC_Quality--; |
*/ |
|
/* Does not belong here. Well since we are not supporting navi right now anyway, leave out. |
#ifdef USE_NAVICTRL |
if(NCDataOkay) { |
if(--NCDataOkay == 0) // no data from NC |
{ // set gps control sticks neutral |
GPSStickPitch = 0; |
GPSStickRoll = 0; |
NCSerialDataOkay = 0; |
} |
} |
#endif |
*/ |
if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout |
RED_ON; |
if (!I2CTimeout) { |
I2C_Reset(); |
I2CTimeout = 5; |
} |
} else { |
RED_OFF; |
} |
|
// Allow Serial Data Transmit if motors must not updated or motors are not running |
if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
usart0_TransmitTxData(); |
} |
|
usart0_ProcessRxData(); |
|
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. |
} else if (UBat < staticParams.LowVoltageWarning) { |
beepBatteryAlarm(); |
} |
|
#ifdef USE_NAVICTRL |
if(NCDataOkay) { |
if(--NCDataOkay == 0) // no data from NC |
{ // set gps control sticks neutral |
GPSStickPitch = 0; |
GPSStickRoll = 0; |
NCSerialDataOkay = 0; |
} |
} |
SPI_StartTransmitPacket(); |
SendSPI = 4; |
#endif |
*/ |
if(!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing ot timeout |
RED_ON; |
if(!I2CTimeout) { |
I2C_Reset(); |
I2CTimeout = 5; |
} |
} else { |
RED_OFF; |
} |
|
// Allow Serial Data Transmit if motors must not updated or motors are not running |
if( !runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
usart0_TransmitTxData(); |
} |
|
usart0_ProcessRxData(); |
|
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. |
} else if(UBat < staticParams.LowVoltageWarning) { |
beepBatteryAlarm(); |
} |
|
timer = SetDelay(20); // every 20 ms |
} |
output_update(); |
} |
|
#ifdef USE_NAVICTRL |
SPI_StartTransmitPacket(); |
SendSPI = 4; |
if(!SendSPI) { |
// SendSPI is decremented in timer0.c with a rate of 9.765 kHz. |
// within the SPI_TransmitByte() routine the value is set to 4. |
// I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz, |
// and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms. |
SPI_TransmitByte(); |
} |
#endif |
timer = SetDelay(20); // every 20 ms |
} |
output_update(); |
} |
|
#ifdef USE_NAVICTRL |
if(!SendSPI) { |
// SendSPI is decremented in timer0.c with a rate of 9.765 kHz. |
// within the SPI_TransmitByte() routine the value is set to 4. |
// I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz, |
// and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms. |
SPI_TransmitByte(); |
} |
#endif |
} |
return (1); |
} |
return (1); |
} |