File content as of revision 25:a9a8e60ff1c2:

#include "Controller.h"
#include <chrono>

using namespace std::chrono;

#define  pi 3.14159265358979323846

Controller::Controller(float Ts) : user_button(PC_13), encoderCounter(PB_6, PB_7),
    u_accx(PA_0), u_accy(PA_1), u_gyro(PA_4), u_i(PA_5),
    bufferedSerial(USBTX, USBRX), thread(osPriorityHigh, 4096)
{
    this->Ts = Ts;

    executeMainTask = false;
    
    user_button.fall(callback(this, &Controller::button_fall));
    user_button.rise(callback(this, &Controller::button_rise));

    /* setup LinearCharacteristics objects for calibration */
    /* measurements for modell D
        - accx: +g: 0.70 V, -g: 0.31 V
        - accy: +g: 0.70 V, -g: 0.31 V
        - gyro: gain: -4.6517f*3.3f, offset: 0.4542 V  */
    u2accx.setup(0.7f, 0.31f, 9.81f, -9.81f);
    u2accy.setup(0.7f, 0.31f, 9.81f, -9.81f);
    u2gyro.setup(-4.6517f*3.3f, 0.4542f);
    /* output maps -15...15 -> 0...1 */
    i2u.setup(-15.0f, 15.0f, 0.0f, 1.0f);

    /* setup variables */
    time = accx = accy = gyro = phi1 = phi2 = dphi2 = M = 0.0f;

    /* setup all IIR_filter objects */
    float tau = 1.0f;
    Gacc .setup(tau, Ts, 1.0f);
    Ggyro.setup(tau, Ts, tau);
    Gdiff.setup(0.01f, Ts);
    Gf   .setup(0.02f, Ts, 1.0f);

    /* setup I-controller object */
    // Ki = -6.8523   -0.8210   -0.0167   -0.0158
    Ci.setup(-0.0158f, Ts, -0.217f*15.0f, 0.217f*15.0f);
    
    bufferedSerial.set_baud(2000000);
    bufferedSerial.set_blocking(false);
    
    /* write_counter_write_val = 5 means every 5th measurement is send via serial */
    write_counter_write_val = 5;

    reset();
    u_i.write( i2u( 0.0f ) );
}

Controller::~Controller()
{
    ticker.detach();
}

void Controller::reset()
{
    loop_timer.reset();
    loop_timer.start();
    time_offset = 0.0f;
    isFirstTimeMeas = true;
    write_counter = write_counter_write_val - 1;
    count_past = encoderCounter.read();
    count = 0;
    Gacc .reset();
    Ggyro.reset();
    Gdiff.reset();
    Gf   .reset();
    Ci.reset();
}

void Controller::start_loop()
{
    thread.start(callback(this, &Controller::loop));
    ticker.attach(callback(this, &Controller::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * Ts)});
}

void Controller::loop()
{
    while(true) {
        ThisThread::flags_wait_any(threadFlag);
        mutex.lock();
        // ----------------------------------------------------------- LOOP START

        /* update time, only used as time reference */
        time = static_cast<float>( duration_cast<microseconds>(loop_timer.elapsed_time()).count() ) * 0.000001f;
        if(isFirstTimeMeas & executeMainTask) {
            time_offset = time;
            isFirstTimeMeas = false;
        }
        time = time - time_offset;

        /* read analog sensors, use LinearCharacteristics for calibration */
        accx = u2accx( u_accx.read() );
        accy = u2accy( u_accy.read() );
        gyro = u2gyro( u_gyro.read() );

        /* complementary filter, use atan2f and IIR_filter for filtering, do not forget the offset */
        phi1 = Gacc(atan2f(accx, accy)) + Ggyro(gyro) - pi/4.0f;

        /* read encoder, short prevents overflow */
        short count_act = encoderCounter.read();
        short delta_count = count_act - count_past;
        count_past = count_act;
        count += delta_count;

        /* calculate angle and angle velocity, use IIR_filter for filtering */
        phi2  = static_cast<float>( count )*2.0f*pi/25600.0f;
        dphi2 = Gdiff(phi2);

        if (executeMainTask) {

            /* controller, use PID_Cntrl for I-controller and IIR_filter for filtering */
            // Ki = -6.8523   -0.8210   -0.0167   -0.0158
            M = Ci(0.0f - dphi2) - (-6.8523*phi1 - 0.8210*Gf( gyro ) - 0.0167*dphi2);
            u_i.write( i2u( M/0.217f ) );

            /* send data via serial */
            if(++write_counter == write_counter_write_val) {
                write_counter = 0;
                /* write output via serial buffer */
                int act_buffer_length = snprintf(buffer, 500,
                                                 "%3.3f, %3.3e, %3.3e, %3.3e, %3.3e, %3.3e, %3.3e, %3.3e\r\n",
                                                 time, accx, accy, gyro, phi1, phi2, dphi2, M);
                bufferedSerial.write(buffer, act_buffer_length);
            }

        } else {
            u_i.write( i2u( 0.0f ) );
        }
        
        // ----------------------------------------------------------- LOOP END
        mutex.unlock();
    }
}

void Controller::sendThreadFlag()
{
    thread.flags_set(threadFlag);
}

void Controller::button_fall()
{
    user_button_timer.reset();
    user_button_timer.start();
}

void Controller::button_rise()
{
    int t_button_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();
    user_button_timer.stop();
    if (t_button_ms > 200) {
        if(executeMainTask) {
            reset();
        }
        executeMainTask = !executeMainTask;
    }
}