| /branches/dongfang_FC_fixedwing/analog.c |
|---|
| 32,9 → 32,8 |
| * the offsets with the DAC. |
| */ |
| volatile uint16_t sensorInputs[8]; |
| //int16_t acc[3]; |
| //int16_t filteredAcc[3] = { 0,0,0 }; |
| /* |
| * These 4 exported variables are zero-offset. The "PID" ones are used |
| * in the attitude control as rotation rates. The "ATT" ones are for |
| 45,8 → 44,6 |
| int16_t gyroD[3]; |
| int16_t gyroDWindow[3][GYRO_D_WINDOW_LENGTH]; |
| uint8_t gyroDWindowIdx = 0; |
| //int16_t dHeight; |
| //uint32_t gyroActivity; |
| /* |
| * Offset values. These are the raw gyro and acc. meter sums when the copter is |
| 53,10 → 50,7 |
| * standing still. They are used for adjusting the gyro and acc. meter values |
| * to be centered on zero. |
| */ |
| sensorOffset_t gyroOffset; |
| sensorOffset_t accOffset; |
| sensorOffset_t gyroAmplifierOffset; |
| /* |
| * In the MK coordinate system, nose-down is positive and left-roll is positive. |
| 124,7 → 118,8 |
| * That is divided by 3 below, for a final 10.34 per volt. |
| * So the initial value of 100 is for 9.7 volts. |
| */ |
| int16_t UBat = 100; |
| uint16_t UBat = 100; |
| uint16_t airspeed = 0; |
| /* |
| * Control and status. |
| /branches/dongfang_FC_fixedwing/analog.h |
|---|
| 60,9 → 60,6 |
| */ |
| #define GYRO_OVERSAMPLING 4 |
| //#define ACC_OVERSAMPLING_XY 2 |
| //#define ACC_OVERSAMPLING_Z 1 |
| /* |
| * The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate. |
| */ |
| 106,17 → 103,17 |
| */ |
| extern int16_t gyro_PID[3]; |
| extern int16_t gyro_ATT[3]; |
| extern int16_t gyroD[3]; |
| #define GYRO_D_WINDOW_LENGTH 8 |
| extern int16_t gyroD[3]; |
| extern int16_t UBat; |
| extern uint16_t UBat; |
| extern uint16_t airspeed; |
| // 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3. |
| #define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3)) |
| extern sensorOffset_t gyroOffset; |
| //extern sensorOffset_t accOffset; |
| //extern sensorOffset_t gyroAmplifierOffset; |
| /* |
| * This is not really for external use - but the ENC-03 gyro modules needs it. |
| 136,7 → 133,6 |
| * its motors running. |
| */ |
| extern uint16_t gyroNoisePeak[3]; |
| //extern uint16_t accNoisePeak[3]; |
| /* |
| * Air pressure. |
| /branches/dongfang_FC_fixedwing/configuration.c |
|---|
| 19,12 → 19,24 |
| VersionInfo_t versionInfo; |
| // MK flags. TODO: Replace by enum. State machine. |
| FlightMode_t currentFlightMode = FLIGHT_MODE_MANUAL; |
| volatile uint8_t isMotorRunning = 0; |
| const MMXLATION XLATIONS[] = { |
| {offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255}, |
| {offsetof(ParamSet_t, gyroPID[0].P), offsetof(DynamicParams_t, gyroPID[0].P),0,255}, |
| {offsetof(ParamSet_t, gyroPID[0].I), offsetof(DynamicParams_t, gyroPID[0].I),0,255}, |
| {offsetof(ParamSet_t, gyroPID[0].D), offsetof(DynamicParams_t, gyroPID[0].D),0,255}, |
| {offsetof(ParamSet_t, gyroPID[1].P), offsetof(DynamicParams_t, gyroPID[1].P),0,255}, |
| {offsetof(ParamSet_t, gyroPID[1].I), offsetof(DynamicParams_t, gyroPID[1].I),0,255}, |
| {offsetof(ParamSet_t, gyroPID[1].D), offsetof(DynamicParams_t, gyroPID[1].D),0,255}, |
| {offsetof(ParamSet_t, gyroPID[2].P), offsetof(DynamicParams_t, gyroPID[2].P),0,255}, |
| {offsetof(ParamSet_t, gyroPID[2].I), offsetof(DynamicParams_t, gyroPID[2].I),0,255}, |
| {offsetof(ParamSet_t, gyroPID[2].D), offsetof(DynamicParams_t, gyroPID[2].D),0,255}, |
| {offsetof(ParamSet_t, servoConfigurations[0].manualControl), offsetof(DynamicParams_t, servoManualControl[0]),0,255}, |
| {offsetof(ParamSet_t, servoConfigurations[1].manualControl), offsetof(DynamicParams_t, servoManualControl[1]),0,255}, |
| {offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255}, |
| 104,7 → 116,8 |
| } |
| void configuration_setFlightParameters(uint8_t newFlightMode) { |
| flight_updateFlightParametersToFlightMode(currentFlightMode = newFlightMode); |
| currentFlightMode = newFlightMode; |
| flight_updateFlightParametersToFlightMode(); |
| } |
| // Called after a change in configuration parameters, as a hook for modules to take over changes. |
| 136,6 → 149,12 |
| staticParams.emergencyThrottle = 35; |
| staticParams.emergencyFlightDuration = 30; |
| for (uint8_t i=0; i<3; i++) { |
| staticParams.gyroPID[i].P = 80; |
| staticParams.gyroPID[i].I = 80; |
| staticParams.gyroPID[i].D = 40; |
| } |
| staticParams.stickIElevator = 80; |
| staticParams.stickIAilerons = 120; |
| staticParams.stickIRudder = 40; |
| 161,7 → 180,7 |
| } |
| staticParams.bitConfig = |
| CFG_GYRO_SATURATION_PREVENTION | CFG_COMPASS_ENABLED; |
| CFG_GYRO_SATURATION_PREVENTION; |
| memcpy(staticParams.name, "Default\0", 6); |
| } |
| /branches/dongfang_FC_fixedwing/configuration.h |
|---|
| 26,6 → 26,39 |
| #define FC_ERROR1_RES2 0x40 |
| #define FC_ERROR1_RES3 0x80 |
| typedef struct { |
| uint8_t gyroQuadrant; |
| uint8_t accQuadrant; |
| uint8_t imuReversedFlags; |
| uint8_t gyroPIDFilterConstant; |
| uint8_t gyroDWindowLength; |
| uint8_t gyroDFilterConstant; |
| uint8_t accFilterConstant; |
| uint8_t zerothOrderCorrection; |
| uint8_t rateTolerance; |
| uint8_t gyroActivityDamping; |
| uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung) |
| uint8_t driftCompLimit; // limit for gyrodrift compensation |
| } IMUConfig_t; |
| extern IMUConfig_t IMUConfig; |
| typedef struct { |
| uint8_t P; |
| uint8_t I; |
| uint8_t D; |
| } PID_t; |
| typedef struct { |
| uint8_t P; |
| uint8_t I; |
| uint8_t D; |
| uint8_t iMax; |
| } PIDIM_t; |
| typedef enum { |
| FLIGHT_MODE_NONE, |
| FLIGHT_MODE_MANUAL, |
| 49,9 → 82,8 |
| extern VersionInfo_t versionInfo; |
| typedef struct { |
| /*PMM*/uint8_t gyroPitchD; |
| /* P */uint8_t gyroRollD;\ |
| /* P */uint8_t gyroYawD; |
| // IMU stuff: |
| PID_t gyroPID[3]; |
| // Control |
| /* P */uint8_t externalControl; |
| 108,45 → 140,13 |
| uint8_t timing; |
| } output_flash_t; |
| typedef struct { |
| uint8_t gyroQuadrant; |
| uint8_t accQuadrant; |
| uint8_t imuReversedFlags; |
| uint8_t gyroPIDFilterConstant; |
| uint8_t gyroDWindowLength; |
| uint8_t gyroDFilterConstant; |
| uint8_t accFilterConstant; |
| uint8_t zerothOrderCorrection; |
| uint8_t rateTolerance; |
| uint8_t gyroActivityDamping; |
| uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung) |
| uint8_t driftCompLimit; // limit for gyrodrift compensation |
| } IMUConfig_t; |
| extern IMUConfig_t IMUConfig; |
| typedef struct { |
| uint8_t P; |
| uint8_t I; |
| uint8_t D; |
| } PID_t; |
| // values above 250 representing poti1 to poti4 |
| typedef struct { |
| // Global bitflags |
| uint8_t bitConfig; // see upper defines for bitcoding |
| // uint8_t axisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
| // uint8_t axisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
| // uint8_t axisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
| uint8_t levelCorrection[2]; |
| // Control |
| PID_t gyroPID[3]; |
| PIDIM_t gyroPID[3]; |
| uint8_t stickIElevator; |
| uint8_t stickIAilerons; |
| 182,14 → 182,8 |
| extern ParamSet_t staticParams; |
| // bit mask for staticParams.bitConfig |
| #define CFG_SIMPLE_HEIGHT_CONTROL (1<<0) |
| #define CFG_SIMPLE_HC_HOLD_SWITCH (1<<1) |
| #define CFG_HEADING_HOLD (1<<2) |
| #define CFG_COMPASS_ENABLED (1<<3) |
| #define CFG_UNUSED (1<<4) |
| #define CFG_NAVI_ENABLED (1<<5) |
| #define CFG_AXIS_COUPLING_ENABLED (1<<6) |
| #define CFG_GYRO_SATURATION_PREVENTION (1<<7) |
| #define CFG_GYRO_SATURATION_PREVENTION (1<<0) |
| #define CFG_USE_AIRSPEED_PID (1<<1) |
| #define IMU_REVERSE_GYRO_PR (1<<0) |
| #define IMU_REVERSE_GYRO_YAW (1<<1) |
| /branches/dongfang_FC_fixedwing/controlMixer.c |
|---|
| 111,7 → 111,6 |
| // Decode commands. |
| uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE; |
| uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE; |
| // Update variables ("potis"). |
| /branches/dongfang_FC_fixedwing/flight.c |
|---|
| 6,11 → 6,11 |
| // Necessary for external control and motor test |
| #include "uart0.h" |
| #include "timer2.h" |
| #include "analog.h" |
| #include "attitude.h" |
| #include "controlMixer.h" |
| #include "configuration.h" |
| #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
| 24,13 → 24,12 |
| */ |
| int32_t error[3]; |
| uint8_t pFactor[3]; |
| uint8_t dFactor[3]; |
| uint8_t iFactor[3]; |
| uint8_t reverse[3]; |
| int32_t maxError[3]; |
| int32_t IPart[3] = { 0, 0, 0 }; |
| PID_t airspeedPID[3]; |
| int16_t controlServos[MAX_CONTROL_SERVOS]; |
| int16_t controlServos[NUM_CONTROL_SERVOS]; |
| /************************************************************************/ |
| /* Neutral Readings */ |
| 44,34 → 43,48 |
| target[YAW] = attitude[YAW]; |
| } |
| // this should be followed by a call to switchToFlightMode!! |
| void flight_updateFlightParametersToFlightMode(uint8_t flightMode) { |
| debugOut.analog[16] = flightMode; |
| void flight_updateFlightParametersToFlightMode(void) { |
| debugOut.analog[16] = currentFlightMode; |
| reverse[PITCH] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_ELEVATOR; |
| reverse[ROLL] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_AILERONS; |
| reverse[YAW] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_RUDDER; |
| reverse[PITCH] = staticParams.controlServosReverse |
| & CONTROL_SERVO_REVERSE_ELEVATOR; |
| reverse[ROLL] = staticParams.controlServosReverse |
| & CONTROL_SERVO_REVERSE_AILERONS; |
| reverse[YAW] = staticParams.controlServosReverse |
| & CONTROL_SERVO_REVERSE_RUDDER; |
| // At a switch to angles, we want to kill errors first. |
| // This should be triggered only once per mode change! |
| if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
| target[PITCH] = attitude[PITCH]; |
| target[ROLL] = attitude[ROLL]; |
| target[YAW] = attitude[YAW]; |
| } |
| for (uint8_t i = 0; i < 3; i++) { |
| if (flightMode == FLIGHT_MODE_MANUAL) { |
| pFactor[i] = 0; |
| dFactor[i] = 0; |
| } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_ANGLES) { |
| pFactor[i] = staticParams.gyroPID[i].P; |
| dFactor[i] = staticParams.gyroPID[i].D; |
| } |
| for (uint8_t axis=0; axis<3; axis++) { |
| maxError[axis] = (int32_t)staticParams.gyroPID[axis].iMax * GYRO_DEG_FACTOR; |
| } |
| } |
| if (flightMode == FLIGHT_MODE_ANGLES) { |
| iFactor[i] = staticParams.gyroPID[i].I; |
| } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_MANUAL) { |
| iFactor[i] = 0; |
| } |
| // Normal at airspeed = 10. |
| uint8_t calcAirspeedPID(uint8_t pid) { |
| if (staticParams.bitConfig & CFG_USE_AIRSPEED_PID) { |
| return pid; |
| } |
| uint16_t result = (pid * 10) / airspeed; |
| if (result > 240 || airspeed == 0) { |
| result = 240; |
| } |
| return result; |
| } |
| void setAirspeedPIDs(void) { |
| for (uint8_t axis = 0; axis<3; axis++) { |
| airspeedPID[axis].P = calcAirspeedPID(dynamicParams.gyroPID[axis].P); |
| airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be??? |
| airspeedPID[axis].D = dynamicParams.gyroPID[axis].D; |
| } |
| } |
| /************************************************************************/ |
| /* Main Flight Control */ |
| /************************************************************************/ |
| 89,10 → 102,8 |
| uint8_t axis; |
| // TODO: Check modern version. |
| // calculateFlightAttitude(); |
| // TODO: Check modern version. |
| // controlMixer_update(); |
| setAirspeedPIDs(); |
| term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
| // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
| 104,7 → 115,7 |
| if (target[axis] > OVER180) { |
| target[axis] -= OVER360; |
| } else if (target[axis] <= -OVER180) { |
| target[axis] += OVER360; |
| target[axis] += OVER360; |
| } |
| if (reverse[axis]) |
| 111,27 → 122,42 |
| error[axis] = attitude[axis] + target[axis]; |
| else |
| error[axis] = attitude[axis] - target[axis]; |
| if (error[axis] > OVER180) { |
| error[axis] -= OVER360; |
| } else if (error[axis] <= -OVER180) { |
| error[axis] += OVER360; |
| if (error[axis] > maxError[axis]) { |
| error[axis] = maxError[axis]; |
| } else if (error[axis] < -maxError[axis]) { |
| error[axis] =- maxError[axis]; |
| } |
| #define LOG_P_SCALE 6 |
| #define LOG_I_SCALE 6 |
| #define LOG_D_SCALE 4 |
| /************************************************************************/ |
| /* Calculate control feedback from angle (gyro integral) */ |
| /* and angular velocity (gyro signal) */ |
| /************************************************************************/ |
| PDPart[axis] = (((int32_t) gyro_PID[axis] * pFactor[axis]) >> 6) |
| + ((gyroD[axis] * (int16_t) dFactor[axis]) >> 4); |
| if (reverse[axis]) |
| PDPart[axis] = -PDPart[axis]; |
| if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode |
| == FLIGHT_MODE_RATE) { |
| PDPart[axis] = (((int32_t) gyro_PID[axis] |
| * (int16_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
| + ((gyroD[axis] * (int16_t) airspeedPID[axis].D) |
| >> LOG_D_SCALE); |
| if (reverse[axis]) |
| PDPart[axis] = -PDPart[axis]; |
| } else { |
| PDPart[axis] = 0; |
| } |
| int16_t anglePart = (error[axis] * iFactor[axis]) >> 10; |
| if (reverse[axis]) |
| PDPart[axis] += anglePart; |
| else |
| PDPart[axis] -= anglePart; |
| if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
| int16_t anglePart = (int32_t)( |
| error[axis] * (int32_t) airspeedPID[axis].I) |
| >> LOG_I_SCALE; |
| if (reverse[axis]) |
| PDPart[axis] += anglePart; |
| else |
| PDPart[axis] -= anglePart; |
| } |
| // Add I parts here... these are integrated errors. |
| // When an error wraps, actually its I part should be negated or something... |
| 143,7 → 169,7 |
| debugOut.analog[14] = term[YAW]; |
| debugOut.analog[15] = term[THROTTLE]; |
| for (uint8_t i = 0; i < MAX_CONTROL_SERVOS; i++) { |
| for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) { |
| int16_t tmp; |
| if (servoTestActive) { |
| controlServos[i] = ((int16_t) servoTest[i] - 128) * 4; |
| /branches/dongfang_FC_fixedwing/flight.h |
|---|
| 14,12 → 14,12 |
| #define YAW 2 |
| #define THROTTLE 3 |
| #define MAX_CONTROL_SERVOS 4 |
| #define NUM_CONTROL_SERVOS 4 |
| extern int16_t controlServos[MAX_CONTROL_SERVOS]; |
| extern int16_t controlServos[NUM_CONTROL_SERVOS]; |
| void flight_control(void); |
| void flight_setNeutral(void); |
| void flight_updateFlightParametersToFlightMode(uint8_t flightMode); |
| void flight_updateFlightParametersToFlightMode(void); |
| #endif //_FLIGHT_H |
| /branches/dongfang_FC_fixedwing/main.c |
|---|
| 109,11 → 109,6 |
| beep(2000); |
| printf("\n\rControl: "); |
| if (staticParams.bitConfig & CFG_HEADING_HOLD) |
| printf("Heading Hold"); |
| else printf("RTL Mode"); |
| printf("\n\n\r"); |
| while (1) { |
| /branches/dongfang_FC_fixedwing/rc.c |
|---|
| 13,8 → 13,6 |
| volatile uint8_t RCQuality; |
| uint8_t lastRCCommand = COMMAND_NONE; |
| uint8_t commandTimer = 0; |
| uint8_t lastFlightMode = FLIGHT_MODE_NONE; |
| /*************************************************************** |
| 176,15 → 174,10 |
| } |
| int16_t channel = RCChannel(CH_THROTTLE); |
| if (channel <= -140) { // <= 900 us |
| if (commandTimer == COMMAND_TIMER) { |
| lastRCCommand = COMMAND_GYROCAL; |
| } |
| if (commandTimer <= COMMAND_TIMER) { |
| commandTimer++; |
| } |
| lastRCCommand = COMMAND_GYROCAL; |
| } else { |
| commandTimer = 0; |
| lastRCCommand = COMMAND_NONE; |
| } |
| return lastRCCommand; |
| 210,17 → 203,6 |
| PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) * RC_SCALING; |
| PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) * RC_SCALING; |
| PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) * RC_SCALING; |
| uint8_t command = COMMAND_NONE; //RC_getStickCommand(); |
| if (lastRCCommand == command) { |
| // Keep timer from overrunning. |
| if (commandTimer < COMMAND_TIMER) |
| commandTimer++; |
| } else { |
| // There was a change. |
| lastRCCommand = command; |
| commandTimer = 0; |
| } |
| } // if RCQuality is no good, we just do nothing. |
| } |
| /branches/dongfang_FC_fixedwing/timer2.c |
|---|
| 4,6 → 4,7 |
| #include "output.h" |
| #include "flight.h" |
| #include "attitude.h" |
| #include "timer2.h" |
| // #define COARSERESOLUTION 1 |
| 22,7 → 23,6 |
| #define CS2 (1<<CS21) |
| #endif |
| #define MAX_SERVOS 8 |
| #define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128) |
| #define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
| #define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
| 144,13 → 144,13 |
| // Save the computation cost of computing a new value before the old one is used. |
| if (!recalculateServoTimes) return; |
| for (axis=0; axis<2; axis++) { |
| value = featuredServoValue(axis); |
| servoValues[axis + 4] = value; |
| for (axis= MAX_CONTROL_SERVOS; axis<MAX_CONTROL_SERVOS+2; axis++) { |
| value = featuredServoValue(axis-MAX_CONTROL_SERVOS); |
| servoValues[axis] = value; |
| } |
| for (axis=2; axis<MAX_SERVOS; axis++) { |
| for (axis=MAX_CONTROL_SERVOS+2; axis<MAX_SERVOS; axis++) { |
| value = 128 * SCALE_FACTOR; |
| servoValues[axis + 4] = value; |
| servoValues[axis] = value; |
| } |
| recalculateServoTimes = 0; |
| /branches/dongfang_FC_fixedwing/timer2.h |
|---|
| 3,8 → 3,8 |
| #include <inttypes.h> |
| extern volatile int16_t ServoNickValue; |
| extern volatile int16_t ServoRollValue; |
| #define MAX_SERVOS 8 |
| #define MAX_CONTROL_SERVOS 4 |
| void calculateControlServoValues(void); |
| void calculateFeaturedServoValues(void); |