File content as of revision 9:1b54bac6d9a7:

#include "sm_esc.h"
#include "sm_sections.h"

PwmOut pwm_ESC(PD_12);
DigitalOut relay_ESC(PG_0);
E_STATE_ESC e_ESC_state;

void init_sm_esc()
{
    e_ESC_state = ESC_DISABLED;
    return;
}

void update_sm_esc()
{
    E_STATE_ESC next_state = e_ESC_state;

    switch(e_ESC_state) {
        case ESC_DISABLED:
            if(b_UTILS_flag_button_ESC && !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_ESC_state = next_state;
    return;
}

void output_sm_esc()
{
    switch(e_ESC_state) {
        case ESC_DISABLED:
            if(b_UTILS_flag_emergency_stop) {
                rs_LOG_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:
            rs_LOG_pc.printf("Init esc\r\n");
            b_UTILS_flag_button_ESC = 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);
            log_start(10000);
            break;

        case ESC_COMMAND:
            switch(e_SECTIONS_state) {
                case RUNNING_SECTION:
                    pwm_ESC.pulsewidth_us(s_UTILS_currentSection->targetSpeed_mps);
                    break;
                case STOPPED:
                    break;
                default:
                    break;
            }
            break;
        default:
            break;
    }

    return;
}