mirror of https://github.com/ArduPilot/ardupilot
125 lines
2.7 KiB
C++
125 lines
2.7 KiB
C++
#include "AnalogIn_Raspilot.h"
|
|
|
|
#include <algorithm>
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include "px4io_protocol.h"
|
|
|
|
AnalogSource_Raspilot::AnalogSource_Raspilot(int16_t pin)
|
|
: _pin(pin)
|
|
{
|
|
}
|
|
|
|
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;
|
|
|
|
AnalogIn_Raspilot::AnalogIn_Raspilot()
|
|
{
|
|
_channels_number = RASPILOT_ADC_MAX_CHANNELS;
|
|
}
|
|
|
|
float AnalogIn_Raspilot::board_voltage(void)
|
|
{
|
|
_vcc_pin_analog_source->set_pin(4);
|
|
|
|
return 5.0;
|
|
}
|
|
|
|
AP_HAL::AnalogSource* AnalogIn_Raspilot::channel(int16_t pin)
|
|
{
|
|
for (uint8_t j = 0; j < _channels_number; j++) {
|
|
if (_channels[j] == nullptr) {
|
|
_channels[j] = new AnalogSource_Raspilot(pin);
|
|
return _channels[j];
|
|
}
|
|
}
|
|
|
|
hal.console->println("Out of analog channels");
|
|
return nullptr;
|
|
}
|
|
|
|
void AnalogIn_Raspilot::init()
|
|
{
|
|
_vcc_pin_analog_source = channel(4);
|
|
|
|
_dev = std::move(hal.spi->get_device("raspio"));
|
|
if (!_dev) {
|
|
AP_HAL::panic("Bus for AnalogIn_Raspilot not found");
|
|
return;
|
|
}
|
|
|
|
_dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AnalogIn_Raspilot::_update, bool));
|
|
}
|
|
|
|
bool AnalogIn_Raspilot::_update()
|
|
{
|
|
struct IOPacket tx = { }, rx = { };
|
|
uint16_t count = RASPILOT_ADC_MAX_CHANNELS;
|
|
tx.count_code = count | PKT_CODE_READ;
|
|
tx.page = PX4IO_PAGE_RAW_ADC_INPUT;
|
|
tx.offset = 0;
|
|
tx.crc = 0;
|
|
tx.crc = crc_packet(&tx);
|
|
|
|
/* set raspilotio to read reg4 */
|
|
_dev->transfer((uint8_t *)&tx, sizeof(tx), (uint8_t *)&rx, sizeof(rx));
|
|
|
|
// TODO: should not delay for such huge values: converting this to a
|
|
// state-machine like driver would be better, adjusting the callback timer
|
|
hal.scheduler->delay_microseconds(200);
|
|
|
|
count = 0;
|
|
tx.count_code = count | PKT_CODE_READ;
|
|
tx.page = 0;
|
|
tx.offset = 0;
|
|
tx.crc = 0;
|
|
tx.crc = crc_packet(&tx);
|
|
|
|
/* get reg4 data from raspilotio */
|
|
_dev->transfer((uint8_t *)&tx, sizeof(tx), (uint8_t *)&rx, sizeof(rx));
|
|
|
|
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 != nullptr && i == source->_pin) {
|
|
source->_value = rx.regs[i] * 3.3 / 4096.0;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|