Momentum-Apps/apps_source_code/ServoTesterApp/app.c

#include <furi.h>
#include <furi_hal.h>
#include <furi_hal_pwm.h>
#include <input/input.h>
#include <gui/gui.h>
#include <stdlib.h>
#include <stm32wbxx_ll_tim.h>
#include <stm32wbxx_ll_lptim.h>
#include <stm32wbxx_ll_rcc.h>

// ./fbt.cmd launch_app APPSRC=servotester

#define SCREEN_XRES 128
#define SCREEN_YRES 64
#define MIN_ANGLE 0
#define MAX_ANGLE 180
#define FREQ 50

typedef enum Mode
{
    Manual = 1,
    Center = 2,
    Sweep = 3,
} Mode;

typedef struct App
{
    Gui *gui;
    ViewPort *view_port;
    FuriMessageQueue *event_queue;

    int running;
    Mode mode;
    uint8_t angle;

    FuriHalPwmOutputId ch_prev;
    FuriHalPwmOutputId ch;
} App;

void input_callback(InputEvent *input_event, void *ctx)
{
    App *app = ctx;
    furi_message_queue_put(app->event_queue, input_event, FuriWaitForever);
}

uint32_t angle_to_compare(uint8_t angle)
{
    /*
        1%:   freq_div: 1280000, prescaler: 19, period: 64000, compare: 640
        3%:   freq_div: 1280000, prescaler: 19, period: 64000, compare: 1920
        5%:   freq_div: 1280000, prescaler: 19, period: 64000, compare: 3200
        7.5%: freq_div: 1280000, prescaler: 19, period: 64000, compare: 4800
        7%:   freq_div: 1280000, prescaler: 19, period: 64000, compare: 4480
        10%:  freq_div: 1280000, prescaler: 19, period: 64000, compare: 6400
        13%:  freq_div: 1280000, prescaler: 19, period: 64000, compare: 8320
        15%:  freq_div: 1280000, prescaler: 19, period: 64000, compare: 9600
        20%:  freq_div: 1280000, prescaler: 19, period: 64000, compare: 12800
    */

    uint32_t min_compare = 1920;
    uint32_t max_compare = 8320;

    if (angle == MIN_ANGLE)
        return min_compare;
    if (angle == MAX_ANGLE)
        return max_compare;

    return min_compare + floor(((float)angle / (float)MAX_ANGLE) * (max_compare - min_compare));
}

void render_callback(Canvas *const canvas, void *ctx)
{
    App *app = ctx;

    canvas_set_color(canvas, ColorWhite);
    canvas_draw_box(canvas, 0, 0, SCREEN_XRES - 1, SCREEN_YRES - 1);
    canvas_set_color(canvas, ColorBlack);
    canvas_set_font(canvas, FontSecondary);

    uint8_t mode_base_x = 20;
    canvas_draw_str(canvas, mode_base_x, 10, "Mode");
    canvas_draw_str(canvas, mode_base_x, 30, "Manual");
    canvas_draw_str(canvas, mode_base_x, 45, "Center");
    canvas_draw_str(canvas, mode_base_x, 60, "Sweep");

    uint8_t selector_base_x = 5;
    uint8_t r = 4;
    uint8_t correction = r;
    if (app->mode == Manual)
    {
        canvas_draw_disc(canvas, selector_base_x, 30 - correction, r);
    }
    else
    {
        canvas_draw_circle(canvas, selector_base_x, 30 - correction, r);
    }

    if (app->mode == Center)
    {
        canvas_draw_disc(canvas, selector_base_x, 45 - correction, r);
    }
    else
    {
        canvas_draw_circle(canvas, selector_base_x, 45 - correction, r);
    }

    if (app->mode == Sweep)
    {
        canvas_draw_disc(canvas, selector_base_x, 60 - correction, r);
    }
    else
    {
        canvas_draw_circle(canvas, selector_base_x, 60 - correction, r);
    }

    uint8_t angle_base_x = 70;
    canvas_draw_str(canvas, angle_base_x, 10, "Angle");
    canvas_draw_circle(canvas, angle_base_x + 18, 21, 2);

    char angle_str[32];
    snprintf(angle_str, sizeof(angle_str), "%3u", app->angle);
    canvas_draw_str(canvas, angle_base_x, 30, angle_str);
}

App *app_alloc()
{
    App *app = malloc(sizeof(App));

    app->gui = furi_record_open(RECORD_GUI);
    app->view_port = view_port_alloc();
    view_port_draw_callback_set(app->view_port, render_callback, app);
    view_port_input_callback_set(app->view_port, input_callback, app);
    gui_add_view_port(app->gui, app->view_port, GuiLayerFullscreen);
    app->event_queue = furi_message_queue_alloc(8, sizeof(InputEvent));

    app->running = 1;
    app->mode = Manual;
    app->ch = FuriHalPwmOutputIdTim1PA7; // A7
    app->angle = 0;

    return app;
}

void app_free(App *app)
{
    furi_assert(app);

    furi_hal_pwm_stop(app->ch);

    view_port_enabled_set(app->view_port, false);
    gui_remove_view_port(app->gui, app->view_port);
    view_port_free(app->view_port);
    furi_record_close(RECORD_GUI);
    furi_message_queue_free(app->event_queue);
    app->gui = NULL;

    free(app);
}

void update_state(App *app, InputKey key)
{
    if (key == InputKeyUp)
    {
        if (app->mode == Manual)
        {
            app->angle = MIN_ANGLE;
            app->mode = Sweep;
        }
        else if (app->mode == Center)
        {
            app->mode = Manual;
        }
        else if (app->mode == Sweep)
        {
            app->mode = Center;
        }
    }

    if (key == InputKeyDown)
    {
        if (app->mode == Manual)
        {
            app->mode = Center;
        }
        else if (app->mode == Center)
        {
            app->angle = MIN_ANGLE;
            app->mode = Sweep;
        }
        else if (app->mode == Sweep)
        {
            app->mode = Manual;
        }
    }

    if (app->mode == Manual)
    {
        if (key == InputKeyRight)
        {
            if (app->angle < MAX_ANGLE)
            {
                app->angle = app->angle + 10;
            }
        }

        if (key == InputKeyLeft)
        {
            if (app->angle > MIN_ANGLE)
            {
                app->angle = app->angle - 10;
            }
        }
    }
}

void custom_pwm_set_params(uint32_t freq, uint32_t compare)
{
    furi_assert(freq > 0);
    uint32_t freq_div = 64000000LU / freq;

    uint32_t prescaler = freq_div / 0x10000LU;
    uint32_t period = freq_div / (prescaler + 1);
    // uint32_t compare = period * duty / 100;

    LL_TIM_SetPrescaler(TIM1, prescaler);
    LL_TIM_SetAutoReload(TIM1, period - 1);
    LL_TIM_OC_SetCompareCH1(TIM1, compare);
}

void tick(void *ctx)
{
    App *app = ctx;

    if (!app->running)
        return;
    if (app->mode != Sweep)
        return;

    if (app->angle == MAX_ANGLE)
    {
        app->angle = MIN_ANGLE;
    }
    else if (app->angle == MIN_ANGLE)
    {
        app->angle = MAX_ANGLE;
    }

    custom_pwm_set_params(FREQ, angle_to_compare(app->angle));
}

int32_t servotester_app_entry(void *p)
{
    UNUSED(p);

    App *app = app_alloc();

    furi_hal_pwm_start(app->ch, FREQ, angle_to_compare(app->angle));
    custom_pwm_set_params(FREQ, angle_to_compare(app->angle));

    FuriTimer *timer = furi_timer_alloc(tick, FuriTimerTypePeriodic, app);
    furi_timer_start(timer, furi_kernel_get_tick_frequency() / 1);

    InputEvent input;
    while (app->running)
    {
        FuriStatus qstat = furi_message_queue_get(app->event_queue, &input, 100);
        if (qstat == FuriStatusOk)
        {
            if (input.key == InputKeyBack)
            {
                app->running = 0;
                // TODO: handle InputTypeLong
            }
            else if (input.type == InputTypePress)
            {
                update_state(app, input.key);

                if (!app->running)
                {
                    break;
                }

                switch (app->mode)
                {
                case Manual:
                    custom_pwm_set_params(FREQ, angle_to_compare(app->angle));
                    break;
                case Center:
                    app->angle = 90;
                    custom_pwm_set_params(FREQ, angle_to_compare(app->angle));
                    break;
                case Sweep:
                    // handled in ticks
                    break;
                }
            }
        }
    }

    furi_timer_free(timer);
    app_free(app);

    return 0;
}