File content as of revision 0:a479dc61e931:

#define PI 3.141592653589793
#include "mbed.h"
#define _MBED_
#include "LinearCharacteristics.h"
#include "UAV.h"
#include "RCin.h"
#include "define_constants.h"
#include "PID_Cntrl.h"
#include "IIR_filter.h"
#include "data_structs.h"
#include "Signal.h"
#include "data_logger.h"
#include "RangeFinder.h"
#include "AHRS.h"
#include "PX4Flow.h"
#include "TFmini.h"

// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
// define IOs
Serial pc(SERIAL_TX, SERIAL_RX,115200);        // serial connection via USB - programmer
RawSerial uart4lidar(PA_0, PA_1);
PwmOut motor_pwms[4];
motor_pwms[0] = new PwmOut(PC_6);
motor_pwms[1] = new PwmOut(PC_8);
motor_pwms[2] = new PwmOut(PB_14);
motor_pwms[3] = new PwmOut(PB_9);
// ----------------------------------------------------------------------------
RCin myRC(PB_6);                // read PWM of RC (Graupner Hott)
// ----------------------------------------------------------------------------
// Ultrasonic Range finder (US), read up to 4 signals 
RangeFinder rf(PA_10, PB_3, PB_5, PB_4 , 10, 5782.0, 0.02, 12000); // 20 Hz parametrization
TFmini tfmini(uart4lidar);
// ----------------------------------------------------------------------------
// PX4 Optical flow (OF), use same i2c like imu
I2C i2c(PC_9, PA_8);
PX4Flow optical_flow(i2c);
// -----------------------------------------------------------------------------
// Timers
Timer global_timer;             // in main loop
// -----------------------------------------------------------------------------
// The copter!
UAV copter(QUAD2,AIRGEAR350);

// -----------------------------------------------------------------------------
// Characteristic for thrust, 
//LinearCharacteristics PM1_2_F_Thrust;       // map +-1 to Thrust
float PM1_2_F_Thrust(float);    // replace linear char. with nonlinear below

// -----------------------------------------------------------------------------
// States and communication
char Message[150];

// -----------------------------------------------------------------------------
// sample rates
float Ts200 = 0.005f;
float Ts100 = 0.01f;
float Ts50 = 0.02f;
float Ts25 = 0.04f;
float Ts10 = 0.1f;
float Ts4 = 0.25f;
// -----------------------------------
float TsCntrl = Ts200;
float TsSDLog = Ts50;
// -----------------------------------
// some stuff
float Mxy_limit = 0.0f;
uint8_t CS=INIT;                // for main state machine
float motorspeed[8];            // motor speeds in rad/sec
AHRS ahrs(1,Ts100,true);        // ahrs filter, 1=EKF,,true=do recalibration of imu

float Kv = 4.0;
// -----------------------------------------------------------------------------
// use pottis for scaling (temporarily)
LinearCharacteristics map_pwm_2_P(864,1600,0.35,1.5);
LinearCharacteristics map_pwm_2_I(864,1600,2.0, 8.0);
LinearCharacteristics map_pwm_2_PX(864,1600,1.0, 4.0);  // RP-Winkelregler
LinearCharacteristics map_pwm_2_scale(864,1600,1.0,2.0);

// -----------------------------------------------------------------------------
// my functions (below)
void updateLoop(void);          // THE MAIN LOOP!!!!
void motor_mix(float *,float ,float *);    // map torques Mx,My,Mz and thrust to motor speeds
void read_US_OF_sensors(void);  // read extra sensors (low frequency)
//
void RPY_controller(float *, uint8_t , uint8_t, float *);   // the main controller for Stabilized and Acro mode
// -----------------------------------------------------------------------------
// - some extra functions
float limit_minmax(float ,float ,float );
float deadzone(float,float,float);
void sendSignal_mainLoop(void);
void sendSignal_read_US_OF_sensors(void);
void sendSignal_Output(void);
// -----------------------------------------------------------------------------
// some values needed
float dt;               // time elapsed in current state
float Mxyz[3];          // torques
float cur_pos[3];
bool disarm_flag=false;
bool flipm = false;     // used while arming motors
float F_thrust;         // Thrust of motors
float z_des = 0.0;
// -----------------------------------------------------------------------------
// ----------- MBED OS ----------------------------
Thread thread_mainLoop(osPriorityNormal, 4096);
Thread thread_read_US_OF_sensors(osPriorityBelowNormal, 4096);
Thread thread_UAV_state(osPriorityLow, 4096);
Thread thread_Output(osPriorityIdle, 4096);
// Signals
Signal signal_mainLoop;
Signal signal_read_US_OF_sensors;
Signal signal_read_OF_sensors;
Signal signal_UAV_state;
Signal signal_Output;
// tickers
Ticker mainLoop;                // Main loop at 100Hz
Ticker US_OF_sensors_loop;
Ticker UAV_state;               // check if UAV crashed / flying / landed etc.
Ticker Output;             // Output loop at 4Hz (Writing and saving)

// -----------------------------------------------------------------------------
// data logger:
data_logger my_logger(35);      // number shows # of logged data columns
// -----------------------------------------------------------------------------
//          MAIN     MAIN     MAIN     MAIN     MAIN     MAIN     MAIN     MAIN 
// -----------------------------------------------------------------------------
int main() {
    i2c.frequency(400000);
    att_cntrl.P_Roll = Kv;   // was 8      // p-controller different attitudes
    att_cntrl.P_Pitch = Kv;
    // --- initialize PWMS
    wait(1);
    uint32_t pwm_init_us = 500;
    PWMOut1.period_ms(5);
    PWMOut1.pulsewidth_us(pwm_init_us);
    PWMOut2.period_ms(5);
    PWMOut2.pulsewidth_us(pwm_init_us);
    PWMOut3.period_ms(5);
    PWMOut3.pulsewidth_us(pwm_init_us);
    PWMOut4.period_ms(5);
    PWMOut4.pulsewidth_us(pwm_init_us);
    copter.calc_copter(myMotor);
    myRC.map_Channels();
    myRC.cou=0;
    myMotor.calc_n2pwm_9k5();
    //myMotor.calc_n2pwm_8k4();
    global_timer.reset();
    global_timer.start();
    wait(1);
    while(!myRC.isAlive) 
        {
        wait(.5);
        pc.printf("Failed to receive RC - signal!\r\n");
        }
    pc.printf("RC - signal on!\r\n");
    wait(.5);
    if(1){
        // start main threads:
        thread_mainLoop.start(&updateLoop);
        mainLoop.attach(&sendSignal_mainLoop, TsCntrl);
        thread_read_US_OF_sensors.start(&read_US_OF_sensors);
        US_OF_sensors_loop.attach(&sendSignal_read_US_OF_sensors,Ts50);
        my_logger.start_logging(TsSDLog);
        my_logger.pause_logging();
        }
}
// -----------------------------------------------------------------------------
// main loop with state handling and controllers
// -----------------------------------------------------------------------------
void updateLoop(void){
    pc.printf("Start Main loop...\r\n");
    bool flightmode_changed = false;
    float old_dt=0;
    uint8_t flm_temp = 1;
    uint8_t flm_loc = 1;
    float des_values[3];    // temporary variable for desired values
    des_values[0]=0.0;    // temporary variable for desired values
    des_values[1]=0.0;    // temporary variable for desired values
    des_values[2]=0.0;    // temporary variable for desired values
    while(1){
        thread_mainLoop.signal_wait(signal_mainLoop);
        old_dt = dt;
        dt  = global_timer.read();              // time elapsed in current state
        my_logger.data_vector[24] = dt - old_dt;    // time delta for analyses 
        if(myRC.flightmode_changed){            // This is triggered bei RC-PWM readout
            flm_temp = myRC.get_flightmode();
            if(flm_temp == VEL_OF_Z_POS & optical_flow.avg_scaled_quality() <150) // do not change to that flightmode, if OF quality ist too low
                {
                 myRC.flightmode_changed = false;   // set flag back, but do nothing else! 
                }
            else{
                flm_loc = flm_temp;
                pc.printf("Changed FM to %d \r\n",flm_loc);
                my_logger.data_vector[23]=(float)flm_loc;
                myRC.flightmode_changed = false;
                flightmode_changed = true;
                }
            }   
        switch(CS)  {
            case INIT:                      // at very beginning
                if (dt > 5.0f && (myRC.PM1[1]<-0.9f) && (myRC.PM1[4]>0.9f)){
                    CS = INIT_MOTORS;              // switch to FLAT state
                    global_timer.reset(); 
                    printf("INIT_MOTORS\r\n");
                    }
                    break;      // CS: INIT
            case INIT_MOTORS:                      // 
                if(dt < 2.0f && flipm){   // after 2 sec armed
                    for(uint8_t m=1;m <= copter.nb_motors;m++)
                        motorspeed[m]=100.0;
                    flipm = false;     
                    }
                else if(dt < 2.0f){   // after 2 sec armed
                    for(uint8_t m=1;m <= copter.nb_motors;m++)
                        motorspeed[m]=0.0f;
                    flipm = true; 
                    }
                else{
                    CS = WAIT_ARMING;
                    global_timer.reset();
                    pc.printf("Motors initialized\r\n");
                    }
                break;      // CS: INIT_MOTORS
            case WAIT_ARMING:                      // 
                if(flm_loc == RTL)    // HACK!: update scaled Parameters
                    {
                    /*rate_cntrl_Roll.scale_PID_param(map_pwm_2_scale(myRC.pwms[10]));
                    rate_cntrl_Pitch.scale_PID_param(map_pwm_2_scale(myRC.pwms[10]));
                    att_cntrl.P_Roll = map_pwm_2_scale(myRC.pwms[11]) * Kv;
                    att_cntrl.P_Pitch = map_pwm_2_scale(myRC.pwms[11]) * Kv;*/
                    my_logger.continue_logging();
                    }
                if((myRC.PM1[1] < -.9) && (myRC.PM1[4]>0.9f) ){      // use CH1 (Roll) and CH4 (Yaw) to arm (stick to middle)
                    CS = ARMING;
                    my_logger.continue_logging();
                    pc.printf("Rate Cntrl P: %2.3f\r\n",rate_cntrl_Roll.get_P_gain());
                    pc.printf("Att Cntrl P: %2.3f\r\n",att_cntrl.P_Roll);
                    global_timer.reset();
                    }
                break;      // CS: WAIT_ARMING
            case ARMING:                      // 
                if((myRC.PM1[1] >= -.9) || (myRC.PM1[4]<.9f)){
                    CS = WAIT_ARMING;
                    global_timer.reset();
                    }
                else if(dt > 1.0f){   // after 1 sec armed
                    CS = START_MOTORS;
                    global_timer.reset();
                    pc.printf("Armed, start motors at %3.0f rad/s \r\n",copter.static_thrust_n*0.1f);
                    }
                break;      // CS: ARMING
            case START_MOTORS:                      // 
                for(uint8_t m=1;m <= copter.nb_motors;m++)
                    motorspeed[m]=copter.static_thrust_n*0.1f;
                if (dt > 1.5f){
                    CS = FLIGHT;
                    global_timer.reset();
                    Mxy_limit = copter.max_Mxy;        
                    rate_cntrl_Roll.reset(0.0);
                    rate_cntrl_Roll.set_limits(-Mxy_limit,Mxy_limit);
                    rate_cntrl_Pitch.reset(0.0);
                    rate_cntrl_Pitch.set_limits(-Mxy_limit,Mxy_limit);
                    // rate_cntrl_Roll_rolloff.reset(0.0);
                    // rate_cntrl_Pitch_rolloff.reset(0.0);
                    rate_cntrl_Yaw.reset(0.0);
                    rate_cntrl_Yaw.set_limits(-copter.max_Mz,copter.max_Mz);
                    pos_cntrl_z.reset(0.0);
                    z_des = 0.0;
                    }
                break;      // CS: START_MOTORS
            case FLIGHT:
                // 2nd SWITCH-CASE
                switch(flm_loc){
                    case STABILIZED:         // STABILIZED: Control RP-Angles with Sticks, Yaw-Rate with Sticks, Thrust with Sticks
                        des_values[0] = myRC.PM1[1] * 0.5f; // +- 1 correspond to +-0.5 rad desired angle 
                        des_values[1] = myRC.PM1[2] * 0.5f; // +- 1 correspond to +-0.5 rad desired angle
                        des_values[2] = -myRC.PM1[4]* 0.8f;//integrate_CH4_2_yaw_angle(myRC.PM1[4]);; // integrate +- 1 to yaw angle
                        F_thrust = PM1_2_F_Thrust(myRC.PM1[3]);    // desired thrust
                        RPY_controller(des_values, 1,1,Mxyz);
                        break;      // flm_loc: STABILIZED
                    case ACRO:       // ACRO: Control Rates with sticks, Thrust with Sticks
                        des_values[0] = myRC.PM1[1]*2.5f; // +- 1 correspond to +-1 rad/s desired rate
                        des_values[1] = myRC.PM1[2]*2.5f; // +- 1 correspond to +-1 rad/s desired rate
                        des_values[2] = -myRC.PM1[4]*1.5f; // +- 1 correspond to +-1 rad/s desired rate
                        F_thrust = PM1_2_F_Thrust(myRC.PM1[3]);    // desired thrust
                        RPY_controller(des_values, 0,1,Mxyz);
                        break;      // flm_loc: ACRO
                    case ALT_HOLD:   // Altitde Hold (with US Sensor)
                        if(flightmode_changed){
                            cur_pos[2] = ahrs.xyzUS[2];
                            pos_cntrl_z.reset(F_thrust);
                            flightmode_changed = false;
                    //        pc.printf("Initialized z:%1.3f\r\n",cur_pos[2]);
                            }
                        des_values[0] = myRC.PM1[1] * 0.5f; // +- 1 correspond to +-0.5 rad desired angle 
                        des_values[1] = myRC.PM1[2] * 0.5f; // +- 1 correspond to +-0.5 rad desired angle
                        des_values[2] = -myRC.PM1[4]* 0.8f; //integrate_CH4_2_yaw_angle(myRC.PM1[4]);; // integrate +- 1 to yaw angle
    //                    cur_pos[2] += 0.8 * TsCntrl * deadzone(myRC.PM1[3],-0.15,0.15); 
    //                    cur_pos[2] = limit_minmax(cur_pos[2],-1.0,0.8);
                        //F_thrust = pos_cntrl_z(cur_pos[2] - ahrs.xyzUS[2]);
                        F_thrust = pos_cntrl_z(0.8f - ahrs.xyzLIDAR[2]);  // use fixed value here and use analog Sensor
                        RPY_controller(des_values, 1,1,Mxyz);
                        break;      // flm_loc: ALT_HOLD
                    case VEL_OF_Z_POS:       // This is the velocity mode with optical flow sensor
                        if(flightmode_changed){
                            vel_cntrl_x.reset(0.0);
                            vel_cntrl_y.reset(0.0);
                            // pos_cntrl_z.reset(F_thrust);     // carfull!!! only valid if this state comes from ALT_HOLD
                            flightmode_changed = false;
                            }
                        des_values[1] = vel_cntrl_x(1.5f * myRC.PM1[2] - ahrs.v_xyzOF[0]);    // Pitchachse: korrekt: positices pitchen führt zu Bewegung in +x
                        des_values[0] = -vel_cntrl_y(-1.5f * myRC.PM1[1] - ahrs.v_xyzOF[1]);;    // Rollachse: Diese ist gedreht: Positives Rollen fuehrt zu Bewegung -y, Hebel nach recht ebenfalls!!!
                        des_values[2] = -myRC.PM1[4]* 0.8f;
                        F_thrust = pos_cntrl_z(0.8f - ahrs.xyzLIDAR[2]);  // use fixed value here and use analog Sensor
                        RPY_controller(des_values, 1,1,Mxyz);
                        break;      // flm_loc: VEL_OF_Z_POS
                    default: break;
                    }       // end of switch(flm_loc)
                // ... continue FLIGHT case
                motor_mix(Mxyz,F_thrust,motorspeed);    // MAp TOrques to Motor speeds
                if((myRC.PM1[1] > 0.8f) && (myRC.PM1[4]<-0.8f) && (myRC.PM1[3]<-0.8f) && disarm_flag == false){      // use CH1 (Roll) and CH4 (Yaw) to disarm (stick to leftright)
                    disarm_flag=true;
                    global_timer.reset();
                    }
                if(disarm_flag == true && dt > 0.8f){
                    disarm_flag=false;
                    global_timer.reset();
                    CS = MOTOR_STOPPED;
                    my_logger.pause_logging();
                    pc.printf("UAV disarmed\r\n");
                    }
                break;  // case FLIGHT
            case MOTOR_STOPPED:
                for(uint8_t m=1;m <= copter.nb_motors;m++)
                    motorspeed[m]=0.0f;
                if(dt>1){
                    global_timer.reset();
                    CS = WAIT_ARMING;
                    }   // case MOTOR_STOPPED
                break;
        default: break;
        }           // end of switch(CS)
        // ------- Datalogger
        my_logger.data_vector[15] = Mxyz[0];
        my_logger.data_vector[16] = Mxyz[1];
        my_logger.data_vector[17] = Mxyz[2];
        my_logger.data_vector[18]=F_thrust;
        my_logger.data_vector[19]=motorspeed[1];
        my_logger.data_vector[20]=motorspeed[2];
        my_logger.data_vector[21]=motorspeed[3];
        my_logger.data_vector[22]=motorspeed[4];
        my_logger.data_vector[25]=des_values[0];
        my_logger.data_vector[26]=des_values[1];
        my_logger.data_vector[27]=des_values[2];
 //       pc.printf("%3.3f %3.3f %3.3f \r\n",des_values[0],des_values[1],des_values[2]);
        }   // of while(1) (the thread)
}
//-----------------------------------------------------------------------------
// Read Ultrasonic range finder Sensors (via PWM) and Optcal Flow Sensors
// WARNING: US-Sensors interfere each other, thus the sensorw are read sequentially
void read_US_OF_sensors(void){
    while(1)
    {
        thread_read_US_OF_sensors.signal_wait(signal_read_US_OF_sensors);
        /*
        rf.read_and_filter(rf.current_sensor++);
        if(rf.current_sensor >= rf.n_sensors)   // loop over 4 Sensors from step to step.(only 1/4 sampling rate)
            rf.current_sensor = 0;
        //for(uint8_t k=0;k<rf.n_sensors;k++)printf("S%d: %1.3f ",k,rf.val[k]);printf("\r\n");
        ahrs.xyzUS[0] = 1.0f-(rf.val[2]+rf.val[3])/2; // mind x positive! If Sensors measure larger distance, it is towards -x !!!
                    // distance 1m is the origin!
        ahrs.xyzUS[1] = rf.val[1]; // larger position of sensor 1 means +y (correct direction)             
        ahrs.xyzUS[2] = rf.val[0]; // z-sensor
        ahrs.rxryrzUS[2] = (rf.val[3]-rf.val[2])/0.4;   // Yaw axis
        my_logger.data_vector[18]=rf.val[0];
        */
        ahrs.xyzAS[2] = 2.311f *exp(-6.5514f*ai1.read())+0.2017f;
        ahrs.xyzLIDAR[2] = tfmini()*0.001f;
        my_logger.data_vector[14]=ahrs.xyzLIDAR[2];
        //my_logger.data_vector[22]=ahrs.xyzAS[2];
        if(optical_flow.update_integral()) {
            ahrs.v_xyzOF[0]= vel_cntrl_x_rolloff(-optical_flow.avg_flowx()*0.001f + ahrs.raw_gy2gy(ahrs.imu.gyroY) * ahrs.xyzLIDAR[2]);
            ahrs.v_xyzOF[1]= vel_cntrl_y_rolloff( optical_flow.avg_flowy()*0.001f - ahrs.raw_gx2gx(ahrs.imu.gyroX) * ahrs.xyzLIDAR[2]);
            ahrs.xyzOF[2]= 0.0f;//((float)optical_flow.ground_distance())*0.001f;         
            my_logger.data_vector[10]=ahrs.v_xyzOF[0];
            my_logger.data_vector[11]=ahrs.v_xyzOF[1];
            my_logger.data_vector[13]=(float)optical_flow.avg_scaled_quality();
            my_logger.data_vector[28]=optical_flow.gyro_x_rate();
            my_logger.data_vector[29]=optical_flow.gyro_y_rate();
            my_logger.data_vector[30]=optical_flow.gyro_z_rate();
            }
        }
}


/* ****************************************************************************
void RPY_controller(float *des_values, uint8_t rate_and_att,float* Mxyz){
 Roll/Pitch/Yaw Controller,
 INPUT:
        des_values      desired angle ( mode stabilized aso), rotation rates (acro) 
        rate_and_att    logic for flight mode (1 = cascade control)
        *Mxyz           resulting values
***************************************************************************** */ 
void RPY_controller(float *des_values, uint8_t rate_and_att,uint8_t filtertype,float* Mxyz){
    float rate_des[4];        
//--------------------------------       
    if(rate_and_att ==1){       // this is for Mode STABILIZED (and Loiter aso)
            rate_des[1] = att_cntrl.P_Roll * (des_values[0] - ahrs.getRoll(filtertype)); 
            rate_des[2] = att_cntrl.P_Pitch * (des_values[1] - ahrs.getPitch(filtertype));
            rate_des[3] = des_values[2];//att_cntrl.P_Yaw * (des_values[2] - ahrs.getYawRadians());
            }
    else{                       // this is for Mode ACRO
            rate_des[1] = des_values[0]; 
            rate_des[2] = des_values[1]; 
            rate_des[3] = des_values[2]; 
        }
    // Mxyz[0]= rate_cntrl_Roll_rolloff( rate_cntrl_Roll(rate_des[1] - ahrs.raw_gx2gx(ahrs.imu.gyroX)) );
    // Mxyz[1]= rate_cntrl_Pitch_rolloff( rate_cntrl_Pitch(rate_des[2] - ahrs.raw_gy2gy(ahrs.imu.gyroY)) );
    float gx = ahrs.raw_gx2gx(ahrs.imu.gyroX);
    float gy = ahrs.raw_gy2gy(ahrs.imu.gyroY);
    Mxyz[0]= rate_cntrl_Roll(rate_des[1] - gx, gx);
    Mxyz[1]= rate_cntrl_Pitch(rate_des[2] - gy, gy);
    Mxyz[2]= rate_cntrl_Yaw(rate_des[3] - ahrs.raw_gz2gz(ahrs.imu.gyroZ));
    }
// *****************************************************************************
// *****************************************************************************
float limit_minmax(float val,float mi,float ma){
    if(val < mi)
        val = mi;
    if(val > ma)
        val = ma;
    return val;
}

// *****************************************************************************
// Dead zone, from lo ... hi
float deadzone(float x,float lo,float hi){
return (x>hi)*(x-hi)+(x<lo)*(x-lo);
}

// *****************************************************************************
// nonlinear characteristics for thrust
float PM1_2_F_Thrust(float x)
{
    return 9.81f*copter.weight*(0.5f*x+1.1f - 0.06f/(x+1.1f));         // y=0.5*x+1.1-0.06./(x+1.1);
}
   

void sendSignal_mainLoop(void){
    thread_mainLoop.signal_set(signal_mainLoop);
}
void sendSignal_read_US_OF_sensors(void){
    thread_read_US_OF_sensors.signal_set(signal_read_US_OF_sensors);
}

void sendSignal_UAV_state(void){
    thread_UAV_state.signal_set(signal_UAV_state);
}
void sendSignal_Output(void){
    thread_Output.signal_set(signal_Output);
}

/*
Thread thread_mainLoop(osPriorityNormal, 4096);
Thread thread_AHRS(osPriorityBelowNormal, 4096);
Thread thread_UAV_state(osPriorityLow, 4096);
Thread thread_Output(osPriorityIdle, 4096);

Signal signal_mainLoop;
Signal signal_AHRS;
Signal signal_UAV_state;
Signal signal_Output;

Ticker AHRS;                   // Madgwick filter for AHRS
Ticker mainLoop;                // Main loop at 200Hz
Ticker UAV_state;               // check if UAV crashed / flying / landed etc.
Ticker Output;             // Output loop at 10Hz (Writing and saving
*/