joystick.c

#include <math.h>
#include <string.h>

#include "config.h"
#include "adc.h"
#include "util.h"
#include "printf.h"
#include "flash.h"
#include "usb_hid.h"
#include "joystick.h"

#if defined(CONFIG_SENSOR_LSM303C) || defined(CONFIG_SENSOR_LSM303DLHC)
#include "lsm303.h"
#endif

#ifdef CONFIG_SENSOR_MLX90393
#include "mlx90393.h"
#endif

#ifdef CONFIG_SENSOR_EMS22A
#include "ems22a.h"
#endif


#define JS_MAX  127
#define JS_MIN -128
 // brake application starts when the rudder is deflected this far from the limit of travel
#define JS_BRAKE_START 32

typedef enum {
    JS_CAL_OFF = 0,
    JS_CAL_ON,
    JS_CAL_END
} js_state_t;


bool_t js_cal_switch = 0;
cal_frame_t cal_frame = {{ "CAL", { 0.0, 1, 1 }, "END" }};
cal_data_t *cal_data = &cal_frame.content.data;


inline static void js_calibrate(float val) {
    static js_state_t state = JS_CAL_OFF;
    static cal_state cs = { 0, 0, 0 };

    val = util_prevent_wraparound(util_normalise(val, cs.offset), &cs);

    switch(state) {
        case JS_CAL_OFF:
            if (!js_cal_switch) return;
            palClearPad(LED_PORT, LED_PIN);
            cs.min = cs.max = val;
            cs.offset = 0;
            state = JS_CAL_ON;
            break;
        case JS_CAL_ON:
            if (!js_cal_switch) state = JS_CAL_END;
            if (cs.min > val) cs.min = val;
            if (cs.max < val) cs.max = val;
            break;
        case JS_CAL_END:
            palSetPad(LED_PORT, LED_PIN);
            cal_data->offset = cs.offset - val;
            cal_data->m_neg = -128 / (cs.min - val);
            cal_data->m_pos = 128 / (cs.max - val);
            cs.min = cs.max = cs.offset = 0;
            state = JS_CAL_OFF;
            break;
    }
}


uint16_t js_read_buttons(void)
{
    uint16_t btns = 0x0;

    for(uint8_t i=0; i<CONFIG_JS_NUM_BUTTONS; i++) {
        btns |= palReadPad(config_button_mappings[i].port, config_button_mappings[i].pad) << i;
    }
    return ~btns;
}


uint16_t js_read_button_matrix(void)
{
    uint16_t btns = 0x0;
#ifdef CONFIG_BUTTON_USE_MATRIX
    uint8_t i, j, k = 0;
    for(i=0; i<CONFIG_JS_MATRIX_SELECT_LINES; i++) {
        palClearPad(config_button_matrix_select[i].port, config_button_matrix_select[i].pad);
        chThdSleepMilliseconds(5);
        for(j=0; j<CONFIG_JS_MATRIX_READ_LINES; j++) {
            /* FIXME: write a flexible button mapping system instead of abusing config_button_mappings.pad */
            btns |= palReadPad(config_button_matrix_read[j].port, config_button_matrix_read[j].pad) << config_button_mappings[k].pad;
            k++;
        }
        palSetPad(config_button_matrix_select[i].port, config_button_matrix_select[i].pad);
    }
#endif
    return ~btns;
}


inline static void js_compute_buttons(uint16_t *btns)
{
    (void) btns;
#ifdef CONFIG_COMPUTE_BUTTON_0_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_0_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_1_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_1_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_2_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_2_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_3_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_3_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_4_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_4_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_5_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_5_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_6_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_6_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_7_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_7_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_8_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_8_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_9_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_9_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_10_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_10_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_11_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_11_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_12_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_12_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_13_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_13_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_14_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_14_EXPR(*btns);
#endif
#ifdef CONFIG_COMPUTE_BUTTON_15_EXPR
        *btns = CONFIG_COMPUTE_BUTTON_15_EXPR(*btns);
#endif

    return;
}


bool js_cal_load(void) {
    BaseSequentialStream *chp = (BaseSequentialStream *)&SDU1;
    cal_frame_t buffer;
    if (!flash_read(buffer.stream, sizeof(buffer.stream))) return false;

    if (strncmp(buffer.content.start, "CAL", 4) || strncmp(buffer.content.end, "END", 4)) {
        printf("ERROR: Calibration data markers don't match\r\n");
        return false;
    }
    chprintf(chp, "M-: %f, M+: %f, offset: %f\r\n", buffer.content.data.m_neg, buffer.content.data.m_pos, buffer.content.data.offset);
    cal_data->m_neg = buffer.content.data.m_neg;
    cal_data->m_pos = buffer.content.data.m_pos;
    cal_data->offset = buffer.content.data.offset;
    return true;
}


inline static float js_cal_apply(float val)
{
    val = util_normalise(val, cal_data->offset);
    if(val < 0) val *= cal_data->m_neg;
    else val *= cal_data->m_pos;
    return util_limit(val);
}


static int8_t js_subrange(float val, uint8_t limit)
{
    float rv = JS_MIN;
    float uval = val - JS_MIN;
    uint8_t threshold = 256 - limit;

    if (uval > threshold)
        rv = (uval - threshold) * 256 / limit + JS_MIN;

    return (int8_t) rv;
}


void js_output(float angle)
{
    js_calibrate(angle);
    angle = js_cal_apply(angle);

#if CONFIG_JS_NUM_AXES > 0
    hid_in_data.fields.axes[0] = (int8_t) angle;
#endif
#if CONFIG_JS_NUM_AXES > 2
    hid_in_data.fields.axes[1] = js_subrange(-angle, JS_BRAKE_START);
    hid_in_data.fields.axes[2] = js_subrange(angle, JS_BRAKE_START);
#endif
}


inline void js_buffer(int16_t *data, int16_t *buf, uint8_t elements, uint8_t factor)
{
    for (uint8_t i = 0; i < elements; i++) {
        buf[i] = (buf[i] * (factor-1) + data[i]) / factor;
    }
}

#ifdef CONFIG_SENSOR_LSM303C
THD_FUNCTION(lsm303c_thread, arg)
{
    I2CDriver *device = (I2CDriver *) arg;
    int16_t data[3];
    int16_t buf[2] = { 0, 0 };

    if(!lsm303c_init(device)) {
        printf("LSM303C not found\r\n");
        chRegSetThreadName("LSM303C fail");
        chThdExit(0);
    }

    while(1) {
        lsm303c_read(device, data);
        js_buffer(data, buf, 2, 8);
        // printf(">L303C %05d %05d %05d\r\n", buf[0], buf[1], buf[2]);
        js_output(util_xy_to_angle(buf[0], buf[1]));
    }
}
#endif

#ifdef CONFIG_SENSOR_LSM303DLHC
THD_FUNCTION(lsm303dlhc_thread, arg)
{
    I2CDriver *device = (I2CDriver *) arg;
    int16_t data[3];
    int16_t buf[2] = { 0, 0 };

    if(!lsm303dlhc_init(device)) {
        printf("LSM303DLHC not found\r\n");
        chRegSetThreadName("LSM303DLHC fail");
        chThdExit(0);
    }

    while(1) {
        lsm303dlhc_read(device, data);
        js_buffer(data, buf, 2, 8);
        // printf(">L303Dx %05d %05d %05d\r\n", buf[0], buf[1], buf[2]);
        js_output(util_xy_to_angle(buf[0], buf[1]));
    }
}
#endif

#ifdef CONFIG_SENSOR_MLX90393
THD_FUNCTION(mlx90393_thread, arg)
{
    I2CDriver *device = (I2CDriver *) arg;
    int16_t data[3];
    int16_t buf[2] = { 0, 0 };

    if(!mlx90393_init(device)) {
        printf("MLX90393 not found\r\n");
        chRegSetThreadName("MLX90393 fail");
        chThdExit(0);
    }

    while(1) {
        mlx90393_read(device, data);
        js_buffer(data, buf, 2, 8);
        // printf("ML393 %05d %05d %05d\r\n", buf[0], buf[1], buf[2]);
        js_output(util_xy_to_angle(buf[0], buf[1]));
    }
}
#endif

#ifdef CONFIG_SENSOR_EMS22A
THD_FUNCTION(ems22a_thread, arg)
{
    SPIDriver *device = (SPIDriver *) arg;
    /*
     * The EMS22A prepends a dummy bit before the start of each 16 bit frame which makes each frame 17 bits each.
     * We must allocate enough extra space the extra bits. One extra 16 bit word gives us space for 16 * 17 bits = 16 daisy chained devices.
     */
    static ems22a_frame rxbuf[EMS22A_NUM_DEVICES+1];

    while (1) {
        ems22a_receive(device, rxbuf, EMS22A_NUM_DEVICES);
        float angle;

        for (uint8_t i = 0; i < EMS22A_NUM_DEVICES; i++) {
           if (rxbuf[i].data.cordic_oflow | rxbuf[i].data.linearity_alarm | rxbuf[i].data.mag_increase | rxbuf[i].data.mag_decrease | rxbuf[i].data.parity) {
                printf("EMS22A error, device %d, value: %04d, error bits: end_offst_comp %d, cordic_oflow %d, linearity_alarm %d, mag_increase %d, mag_decrease %d, parity %d\r\n",
                    i, rxbuf[i].data.value,
                    rxbuf[i].data.end_offst_comp, rxbuf[i].data.cordic_oflow, rxbuf[i].data.linearity_alarm,
                    rxbuf[i].data.mag_increase, rxbuf[i].data.mag_decrease, rxbuf[i].data.parity);
            } else {

                printf(">E22A%d %04d\r\n", i, rxbuf[i].data.value);
                angle = ((float) rxbuf[i].data.value) / 4 - 128;
                js_output(angle);
            }
        }
        chThdSleepMilliseconds(10);
    }
}
#endif


THD_FUNCTION(adc_thread, arg)
{
    (void)arg;

    while(1) {
#if CONFIG_ADC_NUM_INPUTS > 0
        for(uint8_t i=0; i < (uint8_t) CONFIG_ADC_NUM_INPUTS; i++) {
            hid_in_data.fields.axes[adc_axis_mapping[i].axis] = adc_read(adc_axis_mapping[i].input);
        }
#endif

        chThdSleepMilliseconds(10);
    }
}