test
Dependencies: FXOS8700_3 FXAS21002 mbed-rtos kalman MotionSensor EthernetInterface
main.cpp@2:e429f00a3493, 2021-03-28 (annotated)
- Committer:
- fitzpatrick
- Date:
- Sun Mar 28 20:31:29 2021 +0000
- Revision:
- 2:e429f00a3493
- Parent:
- 1:588b597b1c1b
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MACRUM | 0:21d86aae6b2a | 1 | #include "mbed.h" |
fitzpatrick | 1:588b597b1c1b | 2 | #include "FXOS8700.h" |
fitzpatrick | 1:588b597b1c1b | 3 | #include "FXAS21002.h" |
fitzpatrick | 1:588b597b1c1b | 4 | #include "kalman.c" |
fitzpatrick | 1:588b597b1c1b | 5 | #include "EthernetInterface.h" |
fitzpatrick | 1:588b597b1c1b | 6 | |
fitzpatrick | 1:588b597b1c1b | 7 | #define ROLL_SERVO_CENTER 1570 |
fitzpatrick | 1:588b597b1c1b | 8 | #define ROLL_SERVO_MAX 2000 |
fitzpatrick | 1:588b597b1c1b | 9 | #define ROLL_SERVO_MIN 1000 |
fitzpatrick | 1:588b597b1c1b | 10 | |
fitzpatrick | 1:588b597b1c1b | 11 | #define PITCH_SERVO_CENTER 1570 |
fitzpatrick | 1:588b597b1c1b | 12 | #define PITCH_SERVO_MAX 2000 |
fitzpatrick | 1:588b597b1c1b | 13 | #define PITCH_SERVO_MIN 1000 |
fitzpatrick | 1:588b597b1c1b | 14 | |
fitzpatrick | 1:588b597b1c1b | 15 | #define PI 3.14159265 |
fitzpatrick | 1:588b597b1c1b | 16 | #define Rad2Degree 57.2957795 |
fitzpatrick | 1:588b597b1c1b | 17 | |
fitzpatrick | 1:588b597b1c1b | 18 | const int PORT = 7; //arbitrary port |
fitzpatrick | 1:588b597b1c1b | 19 | |
fitzpatrick | 1:588b597b1c1b | 20 | static const char* SERVER_IP = "192.168.1.101"; //IP of server board |
fitzpatrick | 1:588b597b1c1b | 21 | static const char* MASK = "255.255.255.0"; //mask |
fitzpatrick | 1:588b597b1c1b | 22 | static const char* GATEWAY = "192.168.1.1"; //gateway |
fitzpatrick | 1:588b597b1c1b | 23 | |
fitzpatrick | 1:588b597b1c1b | 24 | |
fitzpatrick | 1:588b597b1c1b | 25 | Serial pc(USBTX, USBRX); // Serial Port 115200 |
fitzpatrick | 1:588b597b1c1b | 26 | |
fitzpatrick | 1:588b597b1c1b | 27 | // Sensor GPIO Definitions, I2C |
fitzpatrick | 1:588b597b1c1b | 28 | FXOS8700 accel(D14,D15); |
fitzpatrick | 1:588b597b1c1b | 29 | FXOS8700 mag(D14,D15); |
fitzpatrick | 1:588b597b1c1b | 30 | FXAS21002 gyro(D14,D15); |
fitzpatrick | 1:588b597b1c1b | 31 | |
fitzpatrick | 1:588b597b1c1b | 32 | EthernetInterface eth; //create ethernet |
fitzpatrick | 1:588b597b1c1b | 33 | UDPSocket server; //creat server |
fitzpatrick | 1:588b597b1c1b | 34 | Endpoint client; //create endpoint |
fitzpatrick | 1:588b597b1c1b | 35 | |
fitzpatrick | 1:588b597b1c1b | 36 | //Timer used in Kalman filter |
fitzpatrick | 1:588b597b1c1b | 37 | Timer ProgramTimer; |
MACRUM | 0:21d86aae6b2a | 38 | |
fitzpatrick | 1:588b597b1c1b | 39 | // Servo GPIO Definitions |
fitzpatrick | 1:588b597b1c1b | 40 | PwmOut Pitch_M(D11); |
fitzpatrick | 1:588b597b1c1b | 41 | PwmOut Roll_M(D10); |
fitzpatrick | 1:588b597b1c1b | 42 | |
fitzpatrick | 1:588b597b1c1b | 43 | //Define Kalman filter Parameters |
fitzpatrick | 1:588b597b1c1b | 44 | kalman filter_pitch; |
fitzpatrick | 1:588b597b1c1b | 45 | kalman filter_roll; |
fitzpatrick | 1:588b597b1c1b | 46 | |
fitzpatrick | 1:588b597b1c1b | 47 | float R; |
fitzpatrick | 1:588b597b1c1b | 48 | float Roll_P_C = 1.0f, Roll_I_C = 0.01f, Roll_D_C = 0.01f; //Roll PID coefficient Values |
fitzpatrick | 1:588b597b1c1b | 49 | float Roll_P = 0, Roll_I = 0, Roll_D =0, Roll_Correct = 0, Roll_Error = 0; //Roll PID Control Values |
fitzpatrick | 1:588b597b1c1b | 50 | float Pitch_P_C = 1.0f, Pitch_I_C = 0.01f, Pitch_D_C = 0.01f; //Pitch PID coefficient Values |
fitzpatrick | 1:588b597b1c1b | 51 | float Pitch_P = 0, Pitch_I = 0, Pitch_D = 0, Pitch_Correct = 0, Pitch_Error = 0; //Pitch PID control Values |
fitzpatrick | 1:588b597b1c1b | 52 | int Roll_Servo_Val, Pitch_Servo_Val; // Servo pulse widths |
fitzpatrick | 1:588b597b1c1b | 53 | double angle[3]; // Kalman Pitch and Roll angles |
fitzpatrick | 1:588b597b1c1b | 54 | unsigned long timer; // absolute time stamp |
fitzpatrick | 1:588b597b1c1b | 55 | int val, Loop_Count; // Loop_Count used to settel kalman filter |
fitzpatrick | 1:588b597b1c1b | 56 | |
fitzpatrick | 1:588b597b1c1b | 57 | int n; //size of received message |
fitzpatrick | 1:588b597b1c1b | 58 | char counter[1] = {0}; //sample receive/send buffer |
fitzpatrick | 1:588b597b1c1b | 59 | |
fitzpatrick | 1:588b597b1c1b | 60 | int main() { |
fitzpatrick | 1:588b597b1c1b | 61 | |
fitzpatrick | 1:588b597b1c1b | 62 | pc.baud(115200); // Serial Port 115200 |
fitzpatrick | 1:588b597b1c1b | 63 | |
fitzpatrick | 1:588b597b1c1b | 64 | // Configure Accelerometer FXOS8700, Magnetometer FXOS8700 & Gyroscope FXAS21002 |
fitzpatrick | 1:588b597b1c1b | 65 | accel.accel_config(); |
fitzpatrick | 1:588b597b1c1b | 66 | mag.mag_config(); |
fitzpatrick | 1:588b597b1c1b | 67 | gyro.gyro_config(); |
fitzpatrick | 1:588b597b1c1b | 68 | |
fitzpatrick | 1:588b597b1c1b | 69 | eth.init(SERVER_IP, MASK, GATEWAY); //set up IP |
fitzpatrick | 1:588b597b1c1b | 70 | eth.connect(); //connect ethernet |
fitzpatrick | 1:588b597b1c1b | 71 | printf("\nSERVER - Server IP Address is %s\r\n", eth.getIPAddress()); //get server IP address; |
fitzpatrick | 1:588b597b1c1b | 72 | server.bind(PORT); //bind server |
fitzpatrick | 1:588b597b1c1b | 73 | |
fitzpatrick | 1:588b597b1c1b | 74 | // Initialize Kalman Filter |
fitzpatrick | 1:588b597b1c1b | 75 | kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); |
fitzpatrick | 1:588b597b1c1b | 76 | kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); |
MACRUM | 0:21d86aae6b2a | 77 | |
fitzpatrick | 1:588b597b1c1b | 78 | // Accelerometer, Magnomometer and Gyro Variables |
fitzpatrick | 1:588b597b1c1b | 79 | float adata[3]; //Accelerometer x, y, x values |
fitzpatrick | 1:588b597b1c1b | 80 | float mag_data[3]; //Magnomometer x, y, x values |
fitzpatrick | 1:588b597b1c1b | 81 | float gyro_data[3]; //Gyro pitch, roll, yaw values |
fitzpatrick | 1:588b597b1c1b | 82 | |
fitzpatrick | 1:588b597b1c1b | 83 | // Roll Servo Setup |
fitzpatrick | 1:588b597b1c1b | 84 | int Roll_pulse = ROLL_SERVO_CENTER; |
fitzpatrick | 1:588b597b1c1b | 85 | Roll_M.period_us(20000); // servo requires a 20ms period |
fitzpatrick | 1:588b597b1c1b | 86 | Roll_M.pulsewidth_us(Roll_pulse); // servo position determined by a pulsewidth between 1-2ms |
fitzpatrick | 1:588b597b1c1b | 87 | // Pitch Servo Setup |
fitzpatrick | 1:588b597b1c1b | 88 | int Pitch_pulse = PITCH_SERVO_CENTER; |
fitzpatrick | 1:588b597b1c1b | 89 | Pitch_M.period_us(20000); // servo requires a 20ms period |
fitzpatrick | 1:588b597b1c1b | 90 | Pitch_M.pulsewidth_us(Pitch_pulse); // servo position determined by a pulsewidth between 1-2ms |
fitzpatrick | 1:588b597b1c1b | 91 | wait(1); // Allow servos to settel |
fitzpatrick | 1:588b597b1c1b | 92 | |
fitzpatrick | 1:588b597b1c1b | 93 | //Absolute Time Needed to Calculate Delta Time |
fitzpatrick | 1:588b597b1c1b | 94 | ProgramTimer.start(); |
fitzpatrick | 1:588b597b1c1b | 95 | timer = ProgramTimer.read_us(); |
fitzpatrick | 1:588b597b1c1b | 96 | |
fitzpatrick | 1:588b597b1c1b | 97 | Loop_Count = 15; // Allows Kalman filter to sync. |
fitzpatrick | 1:588b597b1c1b | 98 | |
MACRUM | 0:21d86aae6b2a | 99 | while (true) { |
fitzpatrick | 1:588b597b1c1b | 100 | |
fitzpatrick | 1:588b597b1c1b | 101 | //scanf("%d", &val); // Used for debug |
fitzpatrick | 1:588b597b1c1b | 102 | //printf("\r\n Value is %d \r\n", val); // Used for debug |
fitzpatrick | 1:588b597b1c1b | 103 | |
fitzpatrick | 1:588b597b1c1b | 104 | // Acquire Accelerometer values |
fitzpatrick | 1:588b597b1c1b | 105 | accel.acquire_accel_data_g(adata); // Acquire Acceleration Vectors |
fitzpatrick | 1:588b597b1c1b | 106 | printf("\r\n Accelerometer Values\r\n"); |
fitzpatrick | 1:588b597b1c1b | 107 | printf(" X:%6.1f,\t Y:%6.1f,\t Z:%6.1f\r\n\r\n", adata[0],adata[1],adata[2]); |
fitzpatrick | 1:588b597b1c1b | 108 | |
fitzpatrick | 1:588b597b1c1b | 109 | // Acquire Magnomometer Vectors |
fitzpatrick | 1:588b597b1c1b | 110 | mag.acquire_mag_data_uT(mag_data); // Acquire Magnomometer Vectors |
fitzpatrick | 1:588b597b1c1b | 111 | printf("\r\n Magnomometer Values\r\n"); |
fitzpatrick | 1:588b597b1c1b | 112 | printf(" X:%6.1f,\t Y:%6.1f,\t Z:%6.1f\r\n\r\n", mag_data[0],mag_data[1],mag_data[2]); |
fitzpatrick | 1:588b597b1c1b | 113 | |
fitzpatrick | 1:588b597b1c1b | 114 | // Acquire Gyro values |
fitzpatrick | 1:588b597b1c1b | 115 | gyro.acquire_gyro_data_dps(gyro_data); // Acquire Gyro values |
fitzpatrick | 1:588b597b1c1b | 116 | printf("\r\n Gyro Values\r\n"); |
fitzpatrick | 1:588b597b1c1b | 117 | printf(" Pitch: %4.2f,\t Roll: %4.2f,\t Yaw: %4.2f\r\n", gyro_data[0],gyro_data[1],gyro_data[2]); |
fitzpatrick | 1:588b597b1c1b | 118 | |
fitzpatrick | 1:588b597b1c1b | 119 | // RMS Value of Accelerometer |
fitzpatrick | 1:588b597b1c1b | 120 | R = sqrt(std::pow(adata[0], 2) + std::pow(adata[1], 2) + std::pow(adata[2], 2)); |
fitzpatrick | 1:588b597b1c1b | 121 | |
fitzpatrick | 1:588b597b1c1b | 122 | // Kalman Filter |
fitzpatrick | 1:588b597b1c1b | 123 | kalman_predict(&filter_pitch, gyro_data[0], (ProgramTimer.read_us() - timer)); // Predict uses Pitch portion of gyro and delta time |
fitzpatrick | 1:588b597b1c1b | 124 | kalman_update(&filter_pitch, acos(adata[0]/R)); //Update uses normalized Pitch portion of accelerometer |
fitzpatrick | 1:588b597b1c1b | 125 | kalman_predict(&filter_roll, gyro_data[1], (ProgramTimer.read_us() - timer)); // Predict uses Roll portion of gyro and delta time |
fitzpatrick | 1:588b597b1c1b | 126 | kalman_update(&filter_roll, acos(adata[1]/R)); //Update uses normalized Roll portion of accelerometer |
fitzpatrick | 1:588b597b1c1b | 127 | |
fitzpatrick | 1:588b597b1c1b | 128 | angle[0] = kalman_get_angle(&filter_pitch); // Kalman Pitch |
fitzpatrick | 1:588b597b1c1b | 129 | angle[1] = kalman_get_angle(&filter_roll); // Kalman Roll |
fitzpatrick | 1:588b597b1c1b | 130 | |
fitzpatrick | 1:588b597b1c1b | 131 | timer = ProgramTimer.read_us(); // Update Timer |
fitzpatrick | 1:588b597b1c1b | 132 | |
fitzpatrick | 1:588b597b1c1b | 133 | printf("\r\n Roll Angle X: %.6f Pitch Angle Y: %.6f \r\n", Rad2Degree * angle[1], Rad2Degree * angle[0]); |
fitzpatrick | 1:588b597b1c1b | 134 | |
fitzpatrick | 1:588b597b1c1b | 135 | if (Loop_Count > 0) |
fitzpatrick | 1:588b597b1c1b | 136 | { |
fitzpatrick | 1:588b597b1c1b | 137 | // Loop Iterations to Initialize Kalman Filter to sync. |
fitzpatrick | 1:588b597b1c1b | 138 | Loop_Count--; |
fitzpatrick | 1:588b597b1c1b | 139 | printf("\n\rLoop_Count = %d\n\r", Loop_Count); |
fitzpatrick | 1:588b597b1c1b | 140 | } |
fitzpatrick | 1:588b597b1c1b | 141 | else |
fitzpatrick | 1:588b597b1c1b | 142 | { |
fitzpatrick | 1:588b597b1c1b | 143 | // Loop Iterations Complete, Apply Results of Kalman Filter |
fitzpatrick | 1:588b597b1c1b | 144 | // PID Controller (Mostly Proportional) |
fitzpatrick | 1:588b597b1c1b | 145 | Roll_Error = (Rad2Degree * angle[1]) - 90; // Calculate Roll Error |
fitzpatrick | 1:588b597b1c1b | 146 | Roll_D = Roll_Error - Roll_P; // Differentiation of Roll Error |
fitzpatrick | 1:588b597b1c1b | 147 | Roll_I = Roll_I + Roll_Error; // Intigration of Roll Error |
fitzpatrick | 1:588b597b1c1b | 148 | Roll_P = Roll_Error; // Proportional Roll Error |
fitzpatrick | 1:588b597b1c1b | 149 | Roll_Correct = Roll_P * Roll_P_C + Roll_I * Roll_I_C + Roll_D * Roll_D_C; // Sum product of PID coefficients and values |
fitzpatrick | 1:588b597b1c1b | 150 | printf("\n\rRoll_Correct = %f\n\r",Roll_Correct); |
fitzpatrick | 1:588b597b1c1b | 151 | |
fitzpatrick | 1:588b597b1c1b | 152 | Roll_pulse = Roll_pulse + Roll_Correct; // Update Roll Servo PWM Value |
fitzpatrick | 1:588b597b1c1b | 153 | if (Roll_pulse <= ROLL_SERVO_MIN) Roll_pulse = ROLL_SERVO_MIN; |
fitzpatrick | 1:588b597b1c1b | 154 | if (Roll_pulse >= ROLL_SERVO_MAX) Roll_pulse = ROLL_SERVO_MAX; |
fitzpatrick | 1:588b597b1c1b | 155 | Roll_M.pulsewidth_us(Roll_pulse); // servo position determined by a pulsewidth between 1-2ms |
fitzpatrick | 1:588b597b1c1b | 156 | printf("\n\rRoll_pulse = %d\n\r",Roll_pulse); |
fitzpatrick | 1:588b597b1c1b | 157 | |
fitzpatrick | 1:588b597b1c1b | 158 | Pitch_Error = (Rad2Degree * angle[0]) - 90; // Calculate Pitch Error |
fitzpatrick | 1:588b597b1c1b | 159 | Pitch_D = Pitch_Error - Pitch_P; // Differentiation of Pitch Error |
fitzpatrick | 1:588b597b1c1b | 160 | Pitch_I = Pitch_I + Pitch_Error; // Intigration of Pitch Error |
fitzpatrick | 1:588b597b1c1b | 161 | Pitch_P = Pitch_Error; // Proportional Pitch Error |
fitzpatrick | 1:588b597b1c1b | 162 | Pitch_Correct = Pitch_P * Pitch_P_C + Pitch_I * Pitch_I_C + Pitch_D * Pitch_D_C; // Sum product of PID coefficients and values |
fitzpatrick | 1:588b597b1c1b | 163 | printf("\n\rPitch_Correct = %f\n\r",Pitch_Correct); |
fitzpatrick | 1:588b597b1c1b | 164 | |
fitzpatrick | 1:588b597b1c1b | 165 | Pitch_pulse = Pitch_pulse + Pitch_Correct; // Update Roll Servo PWM Value |
fitzpatrick | 1:588b597b1c1b | 166 | if (Pitch_pulse <= PITCH_SERVO_MIN) Pitch_pulse = PITCH_SERVO_MIN; |
fitzpatrick | 1:588b597b1c1b | 167 | if (Pitch_pulse >= PITCH_SERVO_MAX) Pitch_pulse = PITCH_SERVO_MAX; |
fitzpatrick | 1:588b597b1c1b | 168 | Pitch_M.pulsewidth_us(Pitch_pulse); // servo position determined by a pulsewidth between 1-2ms |
fitzpatrick | 1:588b597b1c1b | 169 | printf("\n\rPitch_pulse = %d\n\r",Pitch_pulse); |
fitzpatrick | 1:588b597b1c1b | 170 | } |
fitzpatrick | 1:588b597b1c1b | 171 | |
fitzpatrick | 1:588b597b1c1b | 172 | wait(0.01); // Loop timer |
fitzpatrick | 1:588b597b1c1b | 173 | } |
fitzpatrick | 1:588b597b1c1b | 174 | |
MACRUM | 0:21d86aae6b2a | 175 | } |
MACRUM | 0:21d86aae6b2a | 176 | |
fitzpatrick | 1:588b597b1c1b | 177 |