Subversion Repositories FlightCtrl

Rev

Rev 2099 | Rev 2103 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2099 Rev 2102
Line 5... Line 5...
5 
#include "output.h"
 5 
#include "output.h"
Line 6... Line 6...
6 
 
 6 
 
7 
// Necessary for external control and motor test
 7 
// Necessary for external control and motor test
Line 8... Line -...
8 
#include "uart0.h"
 - 
 
9 
 
 - 
 
10 
// for scope debugging
 - 
 
11 
// #include "rc.h"
 8 
#include "uart0.h"
12 
 
 9 
 
13 
#include "timer2.h"
 10 
#include "timer2.h"
Line 14... Line 11...
14 
#include "attitude.h"
 11 
#include "attitude.h"
Line 30... Line 27...
30 
uint8_t dFactor[3];
 27 
uint8_t dFactor[3];
31 
uint8_t iFactor[3];
 28 
uint8_t iFactor[3];
32 
uint8_t reverse[3];
 29 
uint8_t reverse[3];
33 
int32_t IPart[3] = { 0, 0, 0 };
 30 
int32_t IPart[3] = { 0, 0, 0 };
Line 34... Line 31...
34 
 
 31 
 
Line 35... Line 32...
35 
int16_t servos[MAX_SERVOS];
 32 
int16_t controlServos[MAX_CONTROL_SERVOS];
36 
 
 33 
 
37 
/************************************************************************/
 34 
/************************************************************************/
38 
/*  Neutral Readings                                                    */
 35 
/*  Neutral Readings                                                    */
Line 39... Line 36...
39 
/************************************************************************/
 36 
/************************************************************************/
40 
#define CONTROL_CONFIG_SCALE 10
 37 
#define CONTROL_CONFIG_SCALE 10
41 
 
 38 
 
42 
void flight_setGround(void) {
 39 
void flight_setGround(void) {
43 
  IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;
 40 
        IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0;
44 
  target[PITCH] = attitude[PITCH];
 41 
        target[PITCH] = attitude[PITCH];
Line 45... Line 42...
45 
  target[ROLL] = attitude[ROLL];
 42 
        target[ROLL] = attitude[ROLL];
46 
  target[YAW] = attitude[YAW];
 43 
        target[YAW] = attitude[YAW];
47 
}
 44 
}
- 
 
 45 
 
48 
 
 46 
// this should be followed by a call to switchToFlightMode!!
49 
void switchToFlightMode(FlightMode_t flightMode) {
 47 
void updateFlightParametersToFlightMode(uint8_t flightMode) {
50 
  if (flightMode == FLIGHT_MODE_MANUAL) {
 48 
        debugOut.analog[15] = flightMode;
51 
    pFactor[PITCH] = 0;
 49 
 
52 
    pFactor[ROLL] = 0;
 50 
        reverse[CONTROL_ELEVATOR] = staticParams.controlServosReverse
53 
    pFactor[YAW] = 0;
 51 
                        & CONTROL_SERVO_REVERSE_ELEVATOR;
54 
  } else if (flightMode == FLIGHT_MODE_RATE) {
 52 
        reverse[CONTROL_AILERONS] = staticParams.controlServosReverse
- 
 
 53 
                        & CONTROL_SERVO_REVERSE_AILERONS;
55 
    pFactor[PITCH] = 0; //staticParams...;
 54 
        reverse[CONTROL_RUDDER] = staticParams.controlServosReverse
56 
    pFactor[ROLL] = 0; //staticParams...;
 - 
 
57 
    pFactor[YAW] = 0; //staticParams...;
 55 
                        & CONTROL_SERVO_REVERSE_RUDDER;
58 
  }
 56 
 
59 
  if (flightMode == FLIGHT_MODE_MANUAL || FLIGHT_MODE_RATE) {
 57 
        for (uint8_t i = 0; i < 3; i++) {
60 
    iFactor[PITCH] = 0;
 - 
 
61 
    iFactor[ROLL] = 0;
 58 
                if (flightMode == FLIGHT_MODE_MANUAL) {
62 
    iFactor[YAW] = 0;
 59 
                        pFactor[i] = 0;
63 
  } else if (flightMode == FLIGHT_MODE_ANGLES) {
 - 
 
64 
    iFactor[PITCH] = 0; //staticParams...;
 - 
 
65 
    iFactor[ROLL] = 0; //staticParams...;
 - 
 
66 
    iFactor[YAW] = 0; //staticParams...;
 - 
 
67 
    // When entering this mode, we want to avoid jerks from accumulated uncorrected errors.
 60 
                        dFactor[i] = 0;
68 
    target[PITCH] = attitude[PITCH];
 61 
                } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_ANGLES) {
69 
    target[ROLL] = attitude[ROLL];
 62 
                        pFactor[i] = staticParams.gyroPID[i].P;
70 
    target[YAW] = attitude[YAW];
 63 
                        dFactor[i] = staticParams.gyroPID[i].D;
71 
  }
 64 
                }
- 
 
 65 
 
- 
 
 66 
                if (flightMode == FLIGHT_MODE_ANGLES) {
72 
 
 67 
                        iFactor[i] = staticParams.gyroPID[i].I;
73 
  dFactor[PITCH] = staticParams.gyroPitchD / CONTROL_CONFIG_SCALE;
 - 
 
74 
  dFactor[ROLL] = staticParams.gyroRollD / CONTROL_CONFIG_SCALE;
 68 
                } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_MANUAL) {
Line 75... Line 69...
75 
  dFactor[YAW] = staticParams.gyroYawD / CONTROL_CONFIG_SCALE;
 69 
                        iFactor[i] = 0;
76 
 
 70 
                }
77 
  // TODO: Set reverse also.
 71 
        }
78 
}
 72 
}
79 
 
 73 
 
80 
/************************************************************************/
 74 
/************************************************************************/
Line 81... Line 75...
81 
/*  Main Flight Control                                                 */
 75 
/*  Main Flight Control                                                 */
82 
/************************************************************************/
 76 
/************************************************************************/
Line 83... Line 77...
83 
void flight_control(void) {
 77 
void flight_control(void) {
Line 192... Line 196...
192 
    debugOut.analog[7] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg
 196 
                debugOut.analog[13] = term[ROLL];