Rev 2112 | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1645 | - | 1 | /*********************************************************************************/ |
| 2 | /* Flight Control */ |
||
| 3 | /*********************************************************************************/ |
||
| 1612 | dongfang | 4 | |
| 5 | #ifndef _FLIGHT_H |
||
| 6 | #define _FLIGHT_H |
||
| 7 | |||
| 8 | #include <inttypes.h> |
||
| 2189 | - | 9 | #include "timer0.h" |
| 1612 | dongfang | 10 | |
| 2189 | - | 11 | typedef enum { |
| 12 | FM_UNINITALIZED, |
||
| 13 | FM_RETURN_TO_LEVEL, |
||
| 14 | FM_HEADING_HOLD, |
||
| 15 | FM_RATE |
||
| 16 | } FlightMode_t; |
||
| 1645 | - | 17 | |
| 2189 | - | 18 | // Max, yaw error to correct. If error is greater, the target heading is pulled towards current heading. |
| 19 | #define YAW_I_LIMIT (int)(PI / 3.0f * INT16DEG_PI_FACTOR) |
||
| 20 | |||
| 21 | // A PI/4 attitude error with average PFactor (PID_NORMAL_VALUE) should make a 1024<<16 diff. |
||
| 22 | // PI/4 * INT16DEG_PI_FACTOR * PID_NORMAL_VALUE * x = (CONTROL_INPUT_HARDWARE_RANGE<<16)/2 --> |
||
| 23 | // x = (CONTROL_INPUT_HARDWARE_RANGE<<15)*4 / (PID_NORMAL_VALUE * PI * INT16DEG_PI_FACTOR) |
||
| 24 | // x = CONTROL_INPUT_HARDWARE_RANGE<<17 / (PID_NORMAL_VALUE * PI * (1<<15) / PI) |
||
| 25 | // x = CONTROL_INPUT_HARDWARE_RANGE<<2 / PID_NORMAL_VALUE = about 82 |
||
| 26 | #define ATT_P_SCALER_LSHIFT16 (int)(((1L<<(LOG_CONTROL_RANGE+2)) / PID_NORMAL_VALUE) + 0.5) |
||
| 27 | |||
| 28 | // A full control input (=half range) with average PFactor (PID_NORMAL_VALUE) should get a rate to drive gyros to PI/sec (say). |
||
| 29 | // PI/s * x * PID_NORMAL_VALUE = CONTROL_INPUT_HARDWARE_RANGE/2 |
||
| 30 | // PI/s is PI * GYRO_RATE_FACTOR_XY units. |
||
| 31 | // PI * GYRO_RATE_FACTOR_XY * x * PID_NORMAL_VALUE = CONTROL_INPUT_HARDWARE_RANGE/2 |
||
| 32 | // x = CONTROL_INPUT_HARDWARE_RANGE/(2*PI*GYRO_RATE_FACTOR_XY*PID_NORMAL_VALUE) = about 379 |
||
| 33 | #define RATE_P_SCALER_LSHIFT16 (int)((1L<<(LOG_CONTROL_RANGE+16)) / (2*PI*GYRO_RATE_FACTOR_XY*PID_NORMAL_VALUE) + 0.5) |
||
| 34 | |||
| 35 | // A full control input (=half range) with average PFactor (PID_NORMAL_VALUE) should get a rate to drive gyros to PI/2sec (say). |
||
| 36 | // PI/2s * x * PID_NORMAL_VALUE = CONTROL_INPUT_HARDWARE_RANGE/2 |
||
| 37 | // PI/2/s is PI/2 * GYRO_RATE_FACTOR_Z units. |
||
| 38 | // PI/2 * GYRO_RATE_FACTOR_Z * x * PID_NORMAL_VALUE = CONTROL_INPUT_HARDWARE_RANGE/2 |
||
| 39 | // PI * GYRO_RATE_FACTOR_Z * x * PID_NORMAL_VALUE = CONTROL_INPUT_HARDWARE_RANGE |
||
| 40 | // x = CONTROL_INPUT_HARDWARE_RANGE / (PI * GYRO_RATE_FACTOR_Z * PID_NORMAL_VALUE) |
||
| 41 | // x<<16 is about 1517 |
||
| 42 | #define YAW_RATE_SCALER_LSHIFT16 (int)((1L<<(LOG_CONTROL_RANGE+16)) / (PI*GYRO_RATE_FACTOR_Z*PID_NORMAL_VALUE) + 0.5) |
||
| 43 | |||
| 44 | // This is where control affects the target heading. It also (later) directly controls yaw. |
||
| 45 | // We want, at a normal yaw stick P, a PI/2s rate at full stick. |
||
| 46 | // That is, f_control * x * CONTROL_INPUT_HARDWARE_RANGE/2 = PI/2 * INT16DEG_PI_FACTOR= 1<<14 |
||
| 47 | // f_control * x * CONTROL_INPUT_HARDWARE_RANGE = 1<<15 |
||
| 48 | // x = 1<<15 / (CONTROL_INPUT_HARDWARE_RANGE * F_CONTROL) |
||
| 49 | // 4194 |
||
| 50 | #define YAW_STICK_INTEGRATION_SCALER_LSHIFT16 (int)(((1L<<(15+16-LOG_CONTROL_RANGE)) / F_CONTROL) + 0.5) |
||
| 51 | |||
| 52 | void flight_setMode(FlightMode_t _flightMode); |
||
| 53 | void flight_setParameters(void); |
||
| 54 | |||
| 2055 | - | 55 | void flight_setGround(void); |
| 56 | void flight_takeOff(void); |
||
| 57 | |||
| 1612 | dongfang | 58 | void flight_control(void); |
| 59 | |||
| 60 | #endif //_FLIGHT_H |