File content as of revision 2:e429f00a3493:

#include "mbed.h"
#include "FXOS8700.h"
#include "FXAS21002.h"
#include "kalman.c"
#include "EthernetInterface.h"

#define ROLL_SERVO_CENTER 1570
#define ROLL_SERVO_MAX 2000
#define ROLL_SERVO_MIN 1000

#define PITCH_SERVO_CENTER 1570
#define PITCH_SERVO_MAX 2000
#define PITCH_SERVO_MIN 1000

#define PI             3.14159265
#define Rad2Degree       57.2957795

const int PORT = 7;                             //arbitrary port

static const char* SERVER_IP = "192.168.1.101"; //IP of server board
static const char* MASK = "255.255.255.0";      //mask
static const char* GATEWAY = "192.168.1.1";     //gateway


Serial pc(USBTX, USBRX); // Serial Port 115200

// Sensor GPIO Definitions, I2C
FXOS8700 accel(D14,D15);
FXOS8700 mag(D14,D15);
FXAS21002 gyro(D14,D15);

EthernetInterface eth;          //create ethernet
UDPSocket server;               //creat server
Endpoint client;                //create endpoint

//Timer used in Kalman filter
Timer ProgramTimer;

// Servo GPIO Definitions
PwmOut Pitch_M(D11);
PwmOut Roll_M(D10);

//Define Kalman filter Parameters
kalman filter_pitch; 
kalman filter_roll;

float R;
float Roll_P_C = 1.0f, Roll_I_C = 0.01f, Roll_D_C = 0.01f; //Roll PID coefficient Values
float Roll_P = 0, Roll_I = 0, Roll_D =0, Roll_Correct = 0, Roll_Error = 0;  //Roll PID Control Values
float Pitch_P_C = 1.0f, Pitch_I_C = 0.01f, Pitch_D_C = 0.01f; //Pitch PID coefficient Values
float Pitch_P = 0, Pitch_I = 0, Pitch_D = 0, Pitch_Correct = 0, Pitch_Error = 0; //Pitch PID control Values
int Roll_Servo_Val, Pitch_Servo_Val;  // Servo pulse widths
double angle[3];      // Kalman Pitch and Roll angles
unsigned long timer;  // absolute time stamp
int val, Loop_Count;  // Loop_Count used to settel kalman filter

int n;                  //size of received message
char counter[1] = {0};  //sample receive/send buffer

int main() {

    pc.baud(115200); // Serial Port 115200
    
    // Configure Accelerometer FXOS8700, Magnetometer FXOS8700 & Gyroscope FXAS21002
    accel.accel_config();
    mag.mag_config();
    gyro.gyro_config();
    
    eth.init(SERVER_IP, MASK, GATEWAY);                                         //set up IP
    eth.connect();                                                              //connect ethernet
    printf("\nSERVER - Server IP Address is %s\r\n", eth.getIPAddress());       //get server IP address;
    server.bind(PORT);                                                          //bind server
    
    // Initialize Kalman Filter
    kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
    kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);

    // Accelerometer, Magnomometer and Gyro Variables
    float adata[3];       //Accelerometer x, y, x values
    float mag_data[3];    //Magnomometer x, y, x values
    float gyro_data[3];   //Gyro pitch, roll, yaw values
    
    // Roll Servo Setup
    int Roll_pulse = ROLL_SERVO_CENTER;
    Roll_M.period_us(20000);          // servo requires a 20ms period
    Roll_M.pulsewidth_us(Roll_pulse); // servo position determined by a pulsewidth between 1-2ms
    // Pitch Servo Setup  
    int Pitch_pulse = PITCH_SERVO_CENTER;
    Pitch_M.period_us(20000);          // servo requires a 20ms period
    Pitch_M.pulsewidth_us(Pitch_pulse); // servo position determined by a pulsewidth between 1-2ms
    wait(1);  // Allow servos to settel 
    
    //Absolute Time Needed to Calculate Delta Time
    ProgramTimer.start();
    timer = ProgramTimer.read_us();
    
    Loop_Count = 15;   // Allows Kalman filter to sync.

    while (true) {

        //scanf("%d", &val);                       // Used for debug
        //printf("\r\n Value is %d \r\n", val);    // Used for debug

        // Acquire Accelerometer values
        accel.acquire_accel_data_g(adata);  // Acquire Acceleration Vectors
        printf("\r\n Accelerometer Values\r\n");
        printf(" X:%6.1f,\t Y:%6.1f,\t Z:%6.1f\r\n\r\n", adata[0],adata[1],adata[2]);
                
        // Acquire Magnomometer Vectors
        mag.acquire_mag_data_uT(mag_data);        // Acquire Magnomometer Vectors
        printf("\r\n Magnomometer Values\r\n");
        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]);    
                
        // Acquire Gyro values
        gyro.acquire_gyro_data_dps(gyro_data);    // Acquire Gyro values
        printf("\r\n Gyro Values\r\n");
        printf(" Pitch: %4.2f,\t Roll: %4.2f,\t Yaw: %4.2f\r\n", gyro_data[0],gyro_data[1],gyro_data[2]);
        
        // RMS Value of Accelerometer 
        R = sqrt(std::pow(adata[0], 2) + std::pow(adata[1], 2) + std::pow(adata[2], 2));
        
        // Kalman Filter
        kalman_predict(&filter_pitch, gyro_data[0],  (ProgramTimer.read_us() - timer)); // Predict uses Pitch portion of gyro and delta time
        kalman_update(&filter_pitch, acos(adata[0]/R));   //Update uses normalized Pitch portion of accelerometer
        kalman_predict(&filter_roll, gyro_data[1],  (ProgramTimer.read_us() - timer)); // Predict uses Roll portion of gyro and delta time
        kalman_update(&filter_roll, acos(adata[1]/R));    //Update uses normalized Roll portion of accelerometer
        
        angle[0] = kalman_get_angle(&filter_pitch);   // Kalman Pitch
        angle[1] = kalman_get_angle(&filter_roll);    // Kalman Roll
        
        timer = ProgramTimer.read_us();     // Update Timer
    
        printf("\r\n Roll Angle X: %.6f   Pitch Angle Y: %.6f \r\n", Rad2Degree * angle[1], Rad2Degree * angle[0]);

        if (Loop_Count > 0) 
        {
            // Loop Iterations to Initialize Kalman Filter to sync.
            Loop_Count--;
            printf("\n\rLoop_Count = %d\n\r", Loop_Count);
        }
        else
        {
            // Loop Iterations Complete, Apply Results of Kalman Filter
            // PID Controller (Mostly Proportional)
            Roll_Error = (Rad2Degree * angle[1]) - 90;  // Calculate Roll Error
            Roll_D = Roll_Error - Roll_P;               // Differentiation of Roll Error
            Roll_I = Roll_I + Roll_Error;               // Intigration of Roll Error
            Roll_P = Roll_Error;                        // Proportional Roll Error
            Roll_Correct = Roll_P * Roll_P_C + Roll_I * Roll_I_C + Roll_D * Roll_D_C;  // Sum product of PID coefficients and values
            printf("\n\rRoll_Correct = %f\n\r",Roll_Correct);       
            
            Roll_pulse =  Roll_pulse + Roll_Correct;    // Update Roll Servo PWM Value
            if (Roll_pulse <= ROLL_SERVO_MIN) Roll_pulse = ROLL_SERVO_MIN;
            if (Roll_pulse >= ROLL_SERVO_MAX) Roll_pulse = ROLL_SERVO_MAX;
            Roll_M.pulsewidth_us(Roll_pulse);           // servo position determined by a pulsewidth between 1-2ms  
            printf("\n\rRoll_pulse = %d\n\r",Roll_pulse);
            
            Pitch_Error = (Rad2Degree * angle[0]) - 90; // Calculate Pitch Error
            Pitch_D = Pitch_Error - Pitch_P;            // Differentiation of Pitch Error
            Pitch_I = Pitch_I + Pitch_Error;            // Intigration of Pitch Error
            Pitch_P = Pitch_Error;                      // Proportional Pitch Error
            Pitch_Correct = Pitch_P * Pitch_P_C + Pitch_I * Pitch_I_C + Pitch_D * Pitch_D_C; // Sum product of PID coefficients and values
            printf("\n\rPitch_Correct = %f\n\r",Pitch_Correct);       

            Pitch_pulse =  Pitch_pulse + Pitch_Correct; // Update Roll Servo PWM Value
            if (Pitch_pulse <= PITCH_SERVO_MIN) Pitch_pulse = PITCH_SERVO_MIN;
            if (Pitch_pulse >= PITCH_SERVO_MAX) Pitch_pulse = PITCH_SERVO_MAX;
            Pitch_M.pulsewidth_us(Pitch_pulse);         // servo position determined by a pulsewidth between 1-2ms  
            printf("\n\rPitch_pulse = %d\n\r",Pitch_pulse);               
        }         
        
        wait(0.01);  // Loop timer
        }

}