File content as of revision 7:cda17cffec3c:

#ifndef QUAD_STATE__H
#define QUAD_STATE__H

#include <stdint.h>

#ifndef QUAD_STATE_NO_JSON
#endif


//this struct must be the same for all the programs communicating with the Quad.
struct QuadState; 
struct QuadState { //double[76]

    double    reference_throttle_1;
    double    reference_throttle_2;
    double    reference_throttle_3;
    double    reference_throttle_4;

    double    actual_throttle_1;
    double    actual_throttle_2;
    double    actual_throttle_3;
    double    actual_throttle_4;

    double    pid_rotation_r_error;
    double    pid_rotation_r_error_integrative;
    double    pid_rotation_r_error_derivative;
    double    pid_rotation_r_kP;
    double    pid_rotation_r_kI;
    double    pid_rotation_r_kD;
    double    pid_rotation_r_out;

    double    pid_rotation_p_error;
    double    pid_rotation_p_error_integrative;
    double    pid_rotation_p_error_derivative;
    double    pid_rotation_p_kP;
    double    pid_rotation_p_kI;
    double    pid_rotation_p_kD;
    double    pid_rotation_p_out;

    double    pid_rotation_y_error;
    double    pid_rotation_y_error_integrative;
    double    pid_rotation_y_error_derivative;
    double    pid_rotation_y_kP;
    double    pid_rotation_y_kI;
    double    pid_rotation_y_kD;
    double    pid_rotation_y_out;

    double    target_main_dt;
    double    actual_main_dt;
    double    average_main_dt;
    double    average_main_dt_k;

    double    target_tx_dt;
    double    actual_tx_dt;
    double    average_tx_dt;
    double    average_tx_dt_k;

    double    target_rx_dt;
    double    actual_rx_dt;
    double    average_rx_dt;
    double    average_rx_dt_k;

    double    target_imu_dt;
    double    actual_imu_dt;
    double    average_imu_dt;
    double    average_imu_dt_k;

    double    estimated_altitude;
    double    estimated_rotation_q1;
    double    estimated_rotation_q2;
    double    estimated_rotation_q3;
    double    estimated_rotation_q4;

    double    estimated_position_x;
    double    estimated_position_y;
    double    estimated_position_z;
    double    estimated_rotation_y;
    double    estimated_rotation_p;
    double    estimated_rotation_r;

    double    sensor_accel_x;
    double    sensor_accel_y;
    double    sensor_accel_z;
    double    sensor_gyro_r;
    double    sensor_gyro_p;
    double    sensor_gyro_y;

    double    target_position_x; //see _isRequired members
    double    target_position_y;
    double    target_position_z;
    double    target_rotation_y;
    double    target_rotation_p;
    double    target_rotation_r;

    double    target_position_x_isRequired; //1=true; 0=false; error otherwise
    double    target_position_y_isRequired;
    double    target_position_z_isRequired;
    double    target_rotation_y_isRequired;
    double    target_rotation_p_isRequired;
    double    target_rotation_r_isRequired;

    double    timestamp;

    double    ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER; //1=OK; 0=EmergencyStop; error otherwise



    static int length () { return 76; }
    
    void reset (){

        reference_throttle_1 = -1000;
        reference_throttle_2 = -1000;
        reference_throttle_3 = -1000;
        reference_throttle_4 = -1000;

        actual_throttle_1 = 0;
        actual_throttle_2 = 0;
        actual_throttle_3 = 0;
        actual_throttle_4 = 0;

        pid_rotation_r_error = 0;
        pid_rotation_r_error_integrative = 0;
        pid_rotation_r_error_derivative = 0;
        pid_rotation_r_kP = 0.3;
        pid_rotation_r_kI = 0;
        pid_rotation_r_kD = 1.2;
        pid_rotation_r_out = 0;

        pid_rotation_p_error = 0;
        pid_rotation_p_error_integrative = 0;
        pid_rotation_p_error_derivative = 0;
        pid_rotation_p_kP = 0.3;
        pid_rotation_p_kI = 0;
        pid_rotation_p_kD = 1.2;
        pid_rotation_p_out = 0;

        pid_rotation_y_error = 0;
        pid_rotation_y_error_integrative = 0;
        pid_rotation_y_error_derivative = 0;
        pid_rotation_y_kP = 0.3;
        pid_rotation_y_kI = 0;
        pid_rotation_y_kD = 0.1;
        pid_rotation_y_out = 0;

        target_main_dt = TARGET_MAIN_DT;
        actual_main_dt = TARGET_MAIN_DT;
        average_main_dt = TARGET_MAIN_DT;
        average_main_dt_k = AVERAGE_DT_K_GAIN;

        target_tx_dt = TARGET_TX_DT;
        actual_tx_dt = TARGET_TX_DT;
        average_tx_dt = TARGET_TX_DT;
        average_tx_dt_k = AVERAGE_DT_K_GAIN;

        target_rx_dt = TARGET_RX_DT;
        actual_rx_dt = TARGET_RX_DT;
        average_rx_dt = TARGET_RX_DT;
        average_rx_dt_k = AVERAGE_DT_K_GAIN;

        target_imu_dt = TARGET_IMU_DT;
        actual_imu_dt = TARGET_IMU_DT;
        average_imu_dt = TARGET_IMU_DT;
        average_imu_dt_k = AVERAGE_DT_K_GAIN;

        estimated_altitude = 0;
        estimated_rotation_q1 = 0;
        estimated_rotation_q2 = 0;
        estimated_rotation_q3 = 0;
        estimated_rotation_q4 = 0;

        estimated_position_x = 0;
        estimated_position_y = 0;
        estimated_position_z = 0;
        estimated_rotation_y = 0;
        estimated_rotation_p = 0;
        estimated_rotation_r = 0;

        sensor_accel_x = 0;
        sensor_accel_y = 0;
        sensor_accel_z = 0;
        sensor_gyro_r = 0;
        sensor_gyro_p = 0;
        sensor_gyro_y = 0;

        target_position_x = 0;
        target_position_y = 0;
        target_position_z = 0;
        target_rotation_y = 0;
        target_rotation_p = 0;
        target_rotation_r = 0;

        target_position_x_isRequired = 0;
        target_position_y_isRequired = 0;
        target_position_z_isRequired = 0;
        target_rotation_y_isRequired = 0;
        target_rotation_p_isRequired = 0;
        target_rotation_r_isRequired = 0;

        timestamp = 0;

        ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0;
    }

#ifndef QUAD_STATE_NO_JSON
    /** Sets the QuadState by parsing a JSON string.
     * @return number of setted members.
     */
    int setFromJSON(const char* JSON_frame){
        /*int setted = 0;
        picojson::value json;
        picojson::parse(json, JSON_frame, JSON_frame + strlen(JSON_frame));

        unsigned int i=0;

//#define JSON_SET(var)  {                                \
                picojson::value j = json.get(i++);      \
                if(j.is<double>()){                     \
                        var = j.get<double>();          \
                        ++setted;                       \
                }                                       \
            }
        */
        
#define JSON_SET(var)  , & var
        
        return sscanf(JSON_frame, "[%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf]"
        
        JSON_SET(reference_throttle_1)
        JSON_SET(reference_throttle_2)
        JSON_SET(reference_throttle_3)
        JSON_SET(reference_throttle_4)

        JSON_SET(actual_throttle_1)
        JSON_SET(actual_throttle_2)
        JSON_SET(actual_throttle_3)
        JSON_SET(actual_throttle_4)

        JSON_SET(pid_rotation_r_error)
        JSON_SET(pid_rotation_r_error_integrative)
        JSON_SET(pid_rotation_r_error_derivative)
        JSON_SET(pid_rotation_r_kP)
        JSON_SET(pid_rotation_r_kI)
        JSON_SET(pid_rotation_r_kD)
        JSON_SET(pid_rotation_r_out)

        JSON_SET(pid_rotation_p_error)
        JSON_SET(pid_rotation_p_error_integrative)
        JSON_SET(pid_rotation_p_error_derivative)
        JSON_SET(pid_rotation_p_kP)
        JSON_SET(pid_rotation_p_kI)
        JSON_SET(pid_rotation_p_kD)
        JSON_SET(pid_rotation_p_out)

        JSON_SET(pid_rotation_y_error)
        JSON_SET(pid_rotation_y_error_integrative)
        JSON_SET(pid_rotation_y_error_derivative)
        JSON_SET(pid_rotation_y_kP)
        JSON_SET(pid_rotation_y_kI)
        JSON_SET(pid_rotation_y_kD)
        JSON_SET(pid_rotation_y_out)

        JSON_SET(target_main_dt)
        JSON_SET(actual_main_dt)
        JSON_SET(average_main_dt)
        JSON_SET(average_main_dt_k)

        JSON_SET(target_tx_dt)
        JSON_SET(actual_tx_dt)
        JSON_SET(average_tx_dt)
        JSON_SET(average_tx_dt_k)

        JSON_SET(target_rx_dt)
        JSON_SET(actual_rx_dt)
        JSON_SET(average_rx_dt)
        JSON_SET(average_rx_dt_k)

        JSON_SET(target_imu_dt)
        JSON_SET(actual_imu_dt)
        JSON_SET(average_imu_dt)
        JSON_SET(average_imu_dt_k)

        JSON_SET(estimated_altitude)
        JSON_SET(estimated_rotation_q1)
        JSON_SET(estimated_rotation_q2)
        JSON_SET(estimated_rotation_q3)
        JSON_SET(estimated_rotation_q4)

        JSON_SET(estimated_position_x)
        JSON_SET(estimated_position_y)
        JSON_SET(estimated_position_z)
        JSON_SET(estimated_rotation_y)
        JSON_SET(estimated_rotation_p)
        JSON_SET(estimated_rotation_r)

        JSON_SET(sensor_accel_x)
        JSON_SET(sensor_accel_y)
        JSON_SET(sensor_accel_z)
        JSON_SET(sensor_gyro_r)
        JSON_SET(sensor_gyro_p)
        JSON_SET(sensor_gyro_y)

        JSON_SET(target_position_x)
        JSON_SET(target_position_y)
        JSON_SET(target_position_z)
        JSON_SET(target_rotation_y)
        JSON_SET(target_rotation_p)
        JSON_SET(target_rotation_r)

        JSON_SET(target_position_x_isRequired)
        JSON_SET(target_position_y_isRequired)
        JSON_SET(target_position_z_isRequired)
        JSON_SET(target_rotation_y_isRequired)
        JSON_SET(target_rotation_p_isRequired)
        JSON_SET(target_rotation_r_isRequired)

        JSON_SET(timestamp)

        JSON_SET(ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER)
        
        );
        
#undef JSON_SET
        
        //return 76;
    }
    
    /** Prints the QuadState in JSON format on the specified buffer.
     * @return total number of characters written. On failure, a negative number is returned.
     */
    int getJSON(char * buffer) const{

        //picojson::array array_vector;

//#define JSON_GET(var)   array_vector.push_back(picojson::value(var));
#define JSON_GET(var) ,var

        return sprintf(buffer, "[%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lg,%lf,%lg]   \r\n"

        JSON_GET(reference_throttle_1)
        JSON_GET(reference_throttle_2)
        JSON_GET(reference_throttle_3)
        JSON_GET(reference_throttle_4)

        JSON_GET(actual_throttle_1)
        JSON_GET(actual_throttle_2)
        JSON_GET(actual_throttle_3)
        JSON_GET(actual_throttle_4)

        JSON_GET(pid_rotation_r_error)
        JSON_GET(pid_rotation_r_error_integrative)
        JSON_GET(pid_rotation_r_error_derivative)
        JSON_GET(pid_rotation_r_kP)
        JSON_GET(pid_rotation_r_kI)
        JSON_GET(pid_rotation_r_kD)
        JSON_GET(pid_rotation_r_out)
        
        JSON_GET(pid_rotation_p_error)
        JSON_GET(pid_rotation_p_error_integrative)
        JSON_GET(pid_rotation_p_error_derivative)
        JSON_GET(pid_rotation_p_kP)
        JSON_GET(pid_rotation_p_kI)
        JSON_GET(pid_rotation_p_kD)
        JSON_GET(pid_rotation_p_out)
        
        JSON_GET(pid_rotation_y_error)
        JSON_GET(pid_rotation_y_error_integrative)
        JSON_GET(pid_rotation_y_error_derivative)
        JSON_GET(pid_rotation_y_kP)
        JSON_GET(pid_rotation_y_kI)
        JSON_GET(pid_rotation_y_kD)
        JSON_GET(pid_rotation_y_out)

        JSON_GET(target_main_dt)
        JSON_GET(actual_main_dt)
        JSON_GET(average_main_dt)
        JSON_GET(average_main_dt_k)

        JSON_GET(target_tx_dt)
        JSON_GET(actual_tx_dt)
        JSON_GET(average_tx_dt)
        JSON_GET(average_tx_dt_k)

        JSON_GET(target_rx_dt)
        JSON_GET(actual_rx_dt)
        JSON_GET(average_rx_dt)
        JSON_GET(average_rx_dt_k)

        JSON_GET(target_imu_dt)
        JSON_GET(actual_imu_dt)
        JSON_GET(average_imu_dt)
        JSON_GET(average_imu_dt_k)

        JSON_GET(estimated_altitude)
        JSON_GET(estimated_rotation_q1)
        JSON_GET(estimated_rotation_q2)
        JSON_GET(estimated_rotation_q3)
        JSON_GET(estimated_rotation_q4)

        JSON_GET(estimated_position_x)
        JSON_GET(estimated_position_y)
        JSON_GET(estimated_position_z)
        JSON_GET(estimated_rotation_y)
        JSON_GET(estimated_rotation_p)
        JSON_GET(estimated_rotation_r)

        JSON_GET(sensor_accel_x)
        JSON_GET(sensor_accel_y)
        JSON_GET(sensor_accel_z)
        JSON_GET(sensor_gyro_r)
        JSON_GET(sensor_gyro_p)
        JSON_GET(sensor_gyro_y)

        JSON_GET(target_position_x)
        JSON_GET(target_position_y)
        JSON_GET(target_position_z)
        JSON_GET(target_rotation_y)
        JSON_GET(target_rotation_p)
        JSON_GET(target_rotation_r)

        JSON_GET(target_position_x_isRequired)
        JSON_GET(target_position_y_isRequired)
        JSON_GET(target_position_z_isRequired)
        JSON_GET(target_rotation_y_isRequired)
        JSON_GET(target_rotation_p_isRequired)
        JSON_GET(target_rotation_r_isRequired)

        JSON_GET(timestamp)

        JSON_GET(ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER)
        
        );
        
#undef JSON_GET

        //return picojson::value(array_vector).serialize();
    }
#endif

};


///Calc dt in seconds.
#define QUAD_STATE_READ_ACTUAL_DT(state, what, timer) {                                                            \
    state.actual_ ## what ## _dt = (double) 0.000001 * timer.read_us();                                            \
}

///Sleep until the setted DT is reached.
#define QUAD_STATE_WAIT_DT_TARGET(actual, target) {                                                                \
    /*if(target-actual > 0.001*target && target-actual > 0.00001){*/                                               \
        Thread::wait((double) 1000.0*target-1000.0*actual);                                                        \
    /*} else*/                                                                                                         \
        /*Thread::yield();*/                                                                                           \
}

///Calc actual dt and update average
#define QUAD_STATE_UPDATE_DT(state, what, timer) {                                                                 \
    state.actual_ ## what ## _dt = (double) 0.000001 * timer.read_us();                                            \
    timer.reset();                                                                                                 \
    state.average_ ## what ## _dt *= 0.9999999999999999 - state.average_ ## what ## _dt_k;                         \
    state.average_ ## what ## _dt += state.actual_ ## what ## _dt * state.average_ ## what ## _dt_k;               \
}


/*
    //Calc dt in seconds
    #define DT_READ() mainQuadState.actual_throttle_dt = (float) (1.0/1000000.0) * dt_t.read_us()
    DT_READ();
    if(MAIN_CYCLE_TARGET_DT - mainQuadState.actual_throttle_dt > MAIN_CYCLE_TARGET_DT/10){
        //if the dt is very small
        //sleep until the setted DT is reached.
        Thread::wait(MAIN_CYCLE_TARGET_DT - mainQuadState.actual_throttle_dt);
        DT_READ();
    }
    dt_timer.reset();

    //Calc average dt
    mainQuadState.average_throttle_dt -= mainQuadState.average_throttle_dt * mainQuadState.average_throttle_dt_k;
    mainQuadState.average_throttle_dt += mainQuadState.actual_throttle_dt * mainQuadState.average_throttle_dt_k;
*/




#endif //QUAD_STATE__H