WebSVN - FlightCtrl - Diff - Rev 2052 and 2055 - /branches/dongfang_FC

-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Copyright (c) 04.2007 Holger Buss
-// + Nur für den privaten Gebrauch
-// + www.MikroKopter.com
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
-// + dass eine Nutzung (auch auszugsweise) nur fï¿½r den privaten und nicht-kommerziellen Gebrauch zulässig ist.
-// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
-// + bzgl. der Nutzungsbedingungen aufzunehmen.
-// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
-// + Verkauf von Luftbildaufnahmen, usw.
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
-// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
-// + auf anderen Webseiten oder Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
-// + eindeutig als Ursprung verlinkt und genannt werden
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
-// + Benutzung auf eigene Gefahr
-// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
-// + mit unserer Zustimmung zulässig
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
-// + this list of conditions and the following disclaimer.
-// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
-// +     from this software without specific prior written permission.
-// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
-// +     for non-commercial use (directly or indirectly)
-// +     Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
-// +     with our written permission
-// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
-// +     clearly linked as origin
-// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
-// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-// +  POSSIBILITY OF SUCH DAMAGE.
-// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include <avr/boot.h>
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <util/delay.h>
 #include "timer0.h"
 #include "timer2.h"
 #include "uart0.h"
 #include "uart1.h"
 #include "output.h"
 #include "menu.h"
 #include "attitude.h"
+#include "commands.h"
 #include "flight.h"
 #include "rc.h"
 #include "analog.h"
 #include "configuration.h"
 #include "printf_P.h"
 #include "twimaster.h"
 #include "controlMixer.h"
 #ifdef USE_NAVICTRL
 #include "spi.h"
 #endif
 #ifdef USE_MK3MAG
 #include "mk3mag.h"
 #endif
 #include "eeprom.h"
 int16_t main(void) {
   uint16_t timer;
   // disable interrupts global
   cli();
   // analyze hardware environment
   CPUType = getCPUType();
   boardRelease = getBoardRelease();
   // disable watchdog
   MCUSR &= ~(1 << WDRF);
   WDTCSR |= (1 << WDCE) | (1 << WDE);
   WDTCSR = 0;
 // This is strange: It should NOT be necessarty to do. But the call of the same,
 // in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,....
 channelMap_default();
   // 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();
 #endif
 #ifdef USE_MK3MAG
   MK3MAG_init();
 #endif
 #ifdef USE_DIRECT_GPS
   usart1_init();
 #endif
   // Parameter Set handling
   channelMap_readOrDefault();
   mixerMatrix_readOrDefault();
   paramSet_readOrDefault();
   // enable interrupts global
   sei();
   printf("\n\r===================================");
   printf("\n\rFlightControl");
   printf("\n\rHardware: Custom");
   printf("\n\r     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));
   // 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);
   outputSet(0,1);
   GRN_OFF;
   RED_ON;
   while (!checkDelay(timer))
+    ;
   timer = setDelay(200);
   outputSet(0,0);
   outputSet(1,1);
   RED_OFF;
   GRN_ON;
   while (!checkDelay(timer))
+    ;
   timer = setDelay(200);
   while (!checkDelay(timer))
+    ;
   outputSet(1,0);
   GRN_OFF;
   twi_diagnostics();
   printf("\n\r===================================");
 #ifdef USE_NAVICTRL
   printf("\n\rSupport for NaviCtrl");
 #endif
 #ifdef USE_DIRECT_GPS
   printf("\n\rDirect (no NaviCtrl) navigation");
 #endif
   controlMixer_setNeutral();
   // Cal. attitude sensors and reset integrals.
   attitude_setNeutral();
   // Init flight parameters
-  flight_setNeutral();
+  // flight_setNeutral();
   beep(2000);
   printf("\n\rControl: ");
   if (staticParams.bitConfig & CFG_HEADING_HOLD)
     printf("Heading Hold");
     else printf("RTL Mode");
   printf("\n\n\r");
   LCD_clear();
   I2CTimeout = 5000;
   while (1) {
     if (runFlightControl) { // control interval
       runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0
       if (!analogDataReady) {
         // Analog data should have been ready but is not!!
         debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER;
       } 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;
         if (!--I2CTimeout || missingMotor) { // try to reset the i2c if motor is missing or timeout
           if (!I2CTimeout) {
             I2C_Reset();
             I2CTimeout = 5;
+          }
           beepI2CAlarm();
+        }
         // 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.batteryVoltageWarning) {
             beepBatteryAlarm();
+          }
 #ifdef USE_NAVICTRL
           SPI_StartTransmitPacket();
           SendSPI = 4;
 #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
           calculateServoValues();
           if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error.
         debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER;
       } else {
         debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER;
+          }
+    }
+  }
   return (1);
+}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/main.c @ 1893 - Rev 2052 → 2055