zr-v4/libraries/AP_HAL_Linux/AnalogIn_Raspilot.cpp

#include <AP_HAL/AP_HAL.h>

#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX

#include "AnalogIn_Raspilot.h"
#include "px4io_protocol.h"

AnalogSource_Raspilot::AnalogSource_Raspilot(int16_t pin):
    _pin(pin),
    _value(0.0f)
{
}

void AnalogSource_Raspilot::set_pin(uint8_t pin)
{
    if (_pin == pin) {
        return;
    }
    _pin = pin;
}

float AnalogSource_Raspilot::read_average()
{
    return read_latest();
}

float AnalogSource_Raspilot::read_latest()
{
    return _value;
}

float AnalogSource_Raspilot::voltage_average()
{
    return _value;
}

float AnalogSource_Raspilot::voltage_latest()
{
    return _value;
}

float AnalogSource_Raspilot::voltage_average_ratiometric()
{
    return _value;
}

extern const AP_HAL::HAL& hal;

RaspilotAnalogIn::RaspilotAnalogIn()
{
    _channels_number = RASPILOT_ADC_MAX_CHANNELS;
}

float RaspilotAnalogIn::board_voltage(void)
{
    _vcc_pin_analog_source->set_pin(4);

    return 5.0;
    //return _vcc_pin_analog_source->voltage_average() * 2.0;
}

AP_HAL::AnalogSource* RaspilotAnalogIn::channel(int16_t pin)
{
    for (uint8_t j = 0; j < _channels_number; j++) {
        if (_channels[j] == NULL) {
            _channels[j] = new AnalogSource_Raspilot(pin);
            return _channels[j];
        }
    }

    hal.console->println("Out of analog channels");
    return NULL;
}

void RaspilotAnalogIn::init()
{
    _vcc_pin_analog_source = channel(4);

    _spi = hal.spi->device(AP_HAL::SPIDevice_RASPIO);
    _spi_sem = _spi->get_semaphore();

    if (_spi_sem == NULL) {
        AP_HAL::panic("PANIC: RCIutput_Raspilot did not get "
                                  "valid SPI semaphore!");
        return; // never reached
    }

    hal.scheduler->suspend_timer_procs();
    hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&RaspilotAnalogIn::_update, void));
    hal.scheduler->resume_timer_procs();
}

void RaspilotAnalogIn::_update()
{
    if (AP_HAL::micros() - _last_update_timestamp < 100000) {
        return;
    }

    if (!_spi_sem->take_nonblocking()) {
        return;
    }

    struct IOPacket _dma_packet_tx, _dma_packet_rx;
    uint16_t count = RASPILOT_ADC_MAX_CHANNELS;
    _dma_packet_tx.count_code = count | PKT_CODE_READ;
    _dma_packet_tx.page = PX4IO_PAGE_RAW_ADC_INPUT;
    _dma_packet_tx.offset = 0;
    _dma_packet_tx.crc = 0;
    _dma_packet_tx.crc = crc_packet(&_dma_packet_tx);
    /* set raspilotio to read reg4 */
    _spi->transaction((uint8_t *)&_dma_packet_tx, (uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_tx));

    hal.scheduler->delay_microseconds(200);

    count = 0;
    _dma_packet_tx.count_code = count | PKT_CODE_READ;
    _dma_packet_tx.page = 0;
    _dma_packet_tx.offset = 0;
    _dma_packet_tx.crc = 0;
    _dma_packet_tx.crc = crc_packet(&_dma_packet_tx);
    /* get reg4 data from raspilotio */
    _spi->transaction((uint8_t *)&_dma_packet_tx, (uint8_t *)&_dma_packet_rx, sizeof(_dma_packet_tx));

    _spi_sem->give();

    for (int16_t i = 0; i < RASPILOT_ADC_MAX_CHANNELS; i++) {
        for (int16_t j=0; j < RASPILOT_ADC_MAX_CHANNELS; j++) {
            AnalogSource_Raspilot *source = _channels[j];

            if (source != NULL && i == source->_pin) {
                source->_value = _dma_packet_rx.regs[i] * 3.3 / 4096.0;
            }
        }
    }

    _last_update_timestamp = AP_HAL::micros();
}

#endif