Subversion Repositories FlightCtrl

Rev

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

Rev 2112 Rev 2189
Line 4... Line 4...
4
 
4
 
5
#ifndef _FLIGHT_H
5
#ifndef _FLIGHT_H
Line 6... Line 6...
6
#define _FLIGHT_H
6
#define _FLIGHT_H
7
 
7
 
8
#include <inttypes.h>
-
 
Line 9... Line 8...
9
#include "analog.h"
8
#include <inttypes.h>
-
 
9
#include "timer0.h"
10
#include "configuration.h"
10
 
-
 
11
typedef enum {
-
 
12
  FM_UNINITALIZED,
11
 
13
  FM_RETURN_TO_LEVEL,
-
 
14
  FM_HEADING_HOLD,
-
 
15
  FM_RATE
-
 
16
} FlightMode_t;
-
 
17
 
-
 
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)
Line 12... Line -...
12
#define PITCH 0
-
 
13
#define ROLL 1
51
 
14
#define YAW 2
52
void flight_setMode(FlightMode_t _flightMode);
Line 15... Line 53...
15
 
53
void flight_setParameters(void);
16
void flight_setParameters(uint8_t _invKi, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor);
-
 
17
void flight_setGround(void);
-
 
Line 18... Line 54...
18
void flight_takeOff(void);
54