File content as of revision 5:8bbe640528bc:

#include "sm_esc.h"

PwmOut pwm_ESC(PD_12);
DigitalOut relay_ESC(PG_0);
E_STATE_ESC e_stateESC;
bool b_chassis_with_pushbutton;

void init_sm_esc()
{
    e_stateESC = ESC_DISABLED;
    if(chassisMode.read()){
        b_chassis_with_pushbutton = true;
        }
        else
        {
            b_chassis_with_pushbutton = false;
        }
    return;
}

void update_sm_esc()
{
    E_STATE_ESC next_state = e_stateESC;

    switch(e_stateESC) {
        case ESC_DISABLED:
            if(b_UTILS_flag_button && !b_UTILS_flag_emergency_stop) {
                next_state = ESC_INIT;
            }
            break;
        case ESC_INIT:
            next_state = ESC_COMMAND;
            break;
        case ESC_COMMAND:
            if(b_UTILS_flag_emergency_stop) {
                next_state = ESC_DISABLED;
            }
            break;
        default:
            break;
    }

    e_stateESC = next_state;
    return;
}

void output_sm_esc()
{
    switch(e_stateESC) {
        case ESC_DISABLED:
            if(b_UTILS_flag_emergency_stop) {
                pc.printf("EMERGENCY STOP\r\n");
                b_UTILS_flag_emergency_stop = false;
                if(b_chassis_with_pushbutton) {
                    relay_ESC = 1;
                    wait(0.5);
                }
                relay_ESC = 0;
            }
            break;

        case ESC_INIT:
            pc.printf("Init esc\r\n");
            b_UTILS_flag_button = false;
            pwm_ESC.period_ms(ESC_PERIOD_DURATION_MS);          //20 ms is default
            pwm_ESC.pulsewidth_us(0);

            relay_ESC = 1;
            if(b_chassis_with_pushbutton) {
            wait(0.5);
            relay_ESC = 0;
            }
            wait(0.1);

            pwm_ESC.pulsewidth_us(ESC_PULSE_MIN_US);
            wait(3);
            pwm_ESC.pulsewidth_us(ESC_PULSE_MAX_US);
            wait(1);
            pwm_ESC.pulsewidth_us(ESC_PULSE_IDLE_US);
            wait(1);
            break;

        case ESC_COMMAND:
            wait(1);
            pwm_ESC.pulsewidth_us(1700);
            wait(0.7);
            pc.printf("send pulse 1620 to esc\r\n");
            pwm_ESC.pulsewidth_us(1620);
            wait(10);

            pc.printf("emergency stop flag raised\r\n");
            b_UTILS_flag_emergency_stop = true;
            break;
        default:
            break;
    }

    return;
}