mirror of https://github.com/ArduPilot/ardupilot
328 lines
9.8 KiB
C++
328 lines
9.8 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#include <AP_HAL.h>
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
|
#include "AnalogIn.h"
|
|
#include <drivers/drv_adc.h>
|
|
#include <stdio.h>
|
|
#include <sys/types.h>
|
|
#include <sys/stat.h>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <nuttx/analog/adc.h>
|
|
#include <nuttx/config.h>
|
|
#include <arch/board/board.h>
|
|
#include <uORB/topics/battery_status.h>
|
|
#include <uORB/topics/servorail_status.h>
|
|
#include <uORB/topics/system_power.h>
|
|
#include <GCS_MAVLink.h>
|
|
#include <errno.h>
|
|
|
|
#define ANLOGIN_DEBUGGING 0
|
|
|
|
// base voltage scaling for 12 bit 3.3V ADC
|
|
#define PX4_VOLTAGE_SCALING (3.3f/4096.0f)
|
|
|
|
#if ANLOGIN_DEBUGGING
|
|
# define Debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
|
|
#else
|
|
# define Debug(fmt, args ...)
|
|
#endif
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
/*
|
|
scaling table between ADC count and actual input voltage, to account
|
|
for voltage dividers on the board.
|
|
*/
|
|
static const struct {
|
|
uint8_t pin;
|
|
float scaling;
|
|
} pin_scaling[] = {
|
|
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
|
// PX4 has 4 FMU analog input pins
|
|
{ 10, (5.7*3.3)/4096 }, // FMU battery on multi-connector pin 5,
|
|
// 5.7:1 scaling
|
|
{ 11, 6.6f/4096 }, // analog airspeed input, 2:1 scaling
|
|
{ 12, 3.3f/4096 }, // analog2, on SPI port pin 3
|
|
{ 13, 16.8f/4096 }, // analog3, on SPI port pin 4
|
|
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
|
|
{ 2, 3.3f/4096 }, // 3DR Brick voltage, usually 10.1:1
|
|
// scaled from battery voltage
|
|
{ 3, 3.3f/4096 }, // 3DR Brick current, usually 17:1 scaled
|
|
// for APM_PER_VOLT
|
|
{ 4, 6.6f/4096 }, // VCC 5V rail sense
|
|
{ 10, 3.3f/4096 }, // spare ADC
|
|
{ 11, 3.3f/4096 }, // spare ADC
|
|
{ 12, 3.3f/4096 }, // spare ADC
|
|
{ 13, 3.3f/4096 }, // AUX ADC pin 4
|
|
{ 14, 3.3f/4096 }, // AUX ADC pin 3
|
|
{ 15, 6.6f/4096 }, // analog airspeed sensor, 2:1 scaling
|
|
#else
|
|
#error "Unknown board type for AnalogIn scaling"
|
|
#endif
|
|
};
|
|
|
|
using namespace PX4;
|
|
|
|
PX4AnalogSource::PX4AnalogSource(int16_t pin, float initial_value) :
|
|
_pin(pin),
|
|
_value(initial_value),
|
|
_value_ratiometric(initial_value),
|
|
_latest_value(initial_value),
|
|
_sum_count(0),
|
|
_sum_value(0),
|
|
_sum_ratiometric(0)
|
|
{
|
|
#ifdef PX4_ANALOG_VCC_5V_PIN
|
|
if (_pin == ANALOG_INPUT_BOARD_VCC) {
|
|
_pin = PX4_ANALOG_VCC_5V_PIN;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
float PX4AnalogSource::read_average()
|
|
{
|
|
if (_sum_count == 0) {
|
|
return _value;
|
|
}
|
|
hal.scheduler->suspend_timer_procs();
|
|
_value = _sum_value / _sum_count;
|
|
_value_ratiometric = _sum_ratiometric / _sum_count;
|
|
_sum_value = 0;
|
|
_sum_ratiometric = 0;
|
|
_sum_count = 0;
|
|
hal.scheduler->resume_timer_procs();
|
|
return _value;
|
|
}
|
|
|
|
float PX4AnalogSource::read_latest()
|
|
{
|
|
return _latest_value;
|
|
}
|
|
|
|
/*
|
|
return scaling from ADC count to Volts
|
|
*/
|
|
float PX4AnalogSource::_pin_scaler(void)
|
|
{
|
|
float scaling = PX4_VOLTAGE_SCALING;
|
|
uint8_t num_scalings = sizeof(pin_scaling)/sizeof(pin_scaling[0]);
|
|
for (uint8_t i=0; i<num_scalings; i++) {
|
|
if (pin_scaling[i].pin == _pin) {
|
|
scaling = pin_scaling[i].scaling;
|
|
break;
|
|
}
|
|
}
|
|
return scaling;
|
|
}
|
|
|
|
/*
|
|
return voltage in Volts
|
|
*/
|
|
float PX4AnalogSource::voltage_average()
|
|
{
|
|
return _pin_scaler() * read_average();
|
|
}
|
|
|
|
/*
|
|
return voltage in Volts, assuming a ratiometric sensor powered by
|
|
the 5V rail
|
|
*/
|
|
float PX4AnalogSource::voltage_average_ratiometric()
|
|
{
|
|
voltage_average();
|
|
return _pin_scaler() * _value_ratiometric;
|
|
}
|
|
|
|
/*
|
|
return voltage in Volts
|
|
*/
|
|
float PX4AnalogSource::voltage_latest()
|
|
{
|
|
return _pin_scaler() * read_latest();
|
|
}
|
|
|
|
void PX4AnalogSource::set_pin(uint8_t pin)
|
|
{
|
|
if (_pin == pin) {
|
|
return;
|
|
}
|
|
hal.scheduler->suspend_timer_procs();
|
|
_pin = pin;
|
|
_sum_value = 0;
|
|
_sum_ratiometric = 0;
|
|
_sum_count = 0;
|
|
_latest_value = 0;
|
|
_value = 0;
|
|
_value_ratiometric = 0;
|
|
hal.scheduler->resume_timer_procs();
|
|
}
|
|
|
|
/*
|
|
apply a reading in ADC counts
|
|
*/
|
|
void PX4AnalogSource::_add_value(float v, float vcc5V)
|
|
{
|
|
_latest_value = v;
|
|
_sum_value += v;
|
|
if (vcc5V < 3.0f) {
|
|
_sum_ratiometric += v;
|
|
} else {
|
|
// this compensates for changes in the 5V rail relative to the
|
|
// 3.3V reference used by the ADC.
|
|
_sum_ratiometric += v * 5.0f / vcc5V;
|
|
}
|
|
_sum_count++;
|
|
if (_sum_count == 254) {
|
|
_sum_value /= 2;
|
|
_sum_ratiometric /= 2;
|
|
_sum_count /= 2;
|
|
}
|
|
}
|
|
|
|
|
|
PX4AnalogIn::PX4AnalogIn() :
|
|
_board_voltage(0),
|
|
_servorail_voltage(0),
|
|
_power_flags(0)
|
|
{}
|
|
|
|
void PX4AnalogIn::init(void* machtnichts)
|
|
{
|
|
_adc_fd = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
|
|
if (_adc_fd == -1) {
|
|
hal.scheduler->panic("Unable to open " ADC_DEVICE_PATH);
|
|
}
|
|
_battery_handle = orb_subscribe(ORB_ID(battery_status));
|
|
_servorail_handle = orb_subscribe(ORB_ID(servorail_status));
|
|
_system_power_handle = orb_subscribe(ORB_ID(system_power));
|
|
}
|
|
|
|
/*
|
|
called at 1kHz
|
|
*/
|
|
void PX4AnalogIn::_timer_tick(void)
|
|
{
|
|
// read adc at 100Hz
|
|
uint32_t now = hal.scheduler->micros();
|
|
uint32_t delta_t = now - _last_run;
|
|
if (delta_t < 10000) {
|
|
return;
|
|
}
|
|
_last_run = now;
|
|
|
|
struct adc_msg_s buf_adc[PX4_ANALOG_MAX_CHANNELS];
|
|
|
|
/* read all channels available */
|
|
int ret = read(_adc_fd, &buf_adc, sizeof(buf_adc));
|
|
if (ret > 0) {
|
|
// match the incoming channels to the currently active pins
|
|
for (uint8_t i=0; i<ret/sizeof(buf_adc[0]); i++) {
|
|
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
|
if (buf_adc[i].am_channel == 4) {
|
|
// record the Vcc value for later use in
|
|
// voltage_average_ratiometric()
|
|
_board_voltage = buf_adc[i].am_data * 6.6f / 4096;
|
|
}
|
|
#endif
|
|
}
|
|
for (uint8_t i=0; i<ret/sizeof(buf_adc[0]); i++) {
|
|
Debug("chan %u value=%u\n",
|
|
(unsigned)buf_adc[i].am_channel,
|
|
(unsigned)buf_adc[i].am_data);
|
|
for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) {
|
|
PX4::PX4AnalogSource *c = _channels[j];
|
|
if (c != NULL && buf_adc[i].am_channel == c->_pin) {
|
|
c->_add_value(buf_adc[i].am_data, _board_voltage);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
|
// check for new battery data on FMUv1
|
|
if (_battery_handle != -1) {
|
|
struct battery_status_s battery;
|
|
bool updated = false;
|
|
if (orb_check(_battery_handle, &updated) == 0 && updated) {
|
|
orb_copy(ORB_ID(battery_status), _battery_handle, &battery);
|
|
if (battery.timestamp != _battery_timestamp) {
|
|
_battery_timestamp = battery.timestamp;
|
|
for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) {
|
|
PX4::PX4AnalogSource *c = _channels[j];
|
|
if (c == NULL) continue;
|
|
if (c->_pin == PX4_ANALOG_ORB_BATTERY_VOLTAGE_PIN) {
|
|
c->_add_value(battery.voltage_v / PX4_VOLTAGE_SCALING, 0);
|
|
}
|
|
if (c->_pin == PX4_ANALOG_ORB_BATTERY_CURRENT_PIN) {
|
|
// scale it back to voltage, knowing that the
|
|
// px4io code scales by 90.0/5.0
|
|
c->_add_value(battery.current_a * (5.0f/90.0f) / PX4_VOLTAGE_SCALING, 0);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
|
|
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
|
// check for new servorail data on FMUv2
|
|
if (_servorail_handle != -1) {
|
|
struct servorail_status_s servorail;
|
|
bool updated = false;
|
|
if (orb_check(_servorail_handle, &updated) == 0 && updated) {
|
|
orb_copy(ORB_ID(servorail_status), _servorail_handle, &servorail);
|
|
if (servorail.timestamp != _servorail_timestamp) {
|
|
_servorail_timestamp = servorail.timestamp;
|
|
_servorail_voltage = servorail.voltage_v;
|
|
for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) {
|
|
PX4::PX4AnalogSource *c = _channels[j];
|
|
if (c == NULL) continue;
|
|
if (c->_pin == PX4_ANALOG_ORB_SERVO_VOLTAGE_PIN) {
|
|
c->_add_value(servorail.voltage_v / PX4_VOLTAGE_SCALING, 0);
|
|
}
|
|
if (c->_pin == PX4_ANALOG_ORB_SERVO_VRSSI_PIN) {
|
|
c->_add_value(servorail.rssi_v / PX4_VOLTAGE_SCALING, 0);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if (_system_power_handle != -1) {
|
|
struct system_power_s system_power;
|
|
bool updated = false;
|
|
if (orb_check(_system_power_handle, &updated) == 0 && updated) {
|
|
orb_copy(ORB_ID(system_power), _system_power_handle, &system_power);
|
|
uint16_t flags = 0;
|
|
if (system_power.usb_connected) flags |= MAV_POWER_STATUS_USB_CONNECTED;
|
|
if (system_power.brick_valid) flags |= MAV_POWER_STATUS_BRICK_VALID;
|
|
if (system_power.servo_valid) flags |= MAV_POWER_STATUS_SERVO_VALID;
|
|
if (system_power.periph_5V_OC) flags |= MAV_POWER_STATUS_PERIPH_OVERCURRENT;
|
|
if (system_power.hipower_5V_OC) flags |= MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT;
|
|
if (_power_flags != 0 && _power_flags != flags) {
|
|
// the power status has changed since boot
|
|
flags |= MAV_POWER_STATUS_CHANGED;
|
|
}
|
|
_power_flags = flags;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
}
|
|
|
|
AP_HAL::AnalogSource* PX4AnalogIn::channel(int16_t pin)
|
|
{
|
|
for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) {
|
|
if (_channels[j] == NULL) {
|
|
_channels[j] = new PX4AnalogSource(pin, 0.0);
|
|
return _channels[j];
|
|
}
|
|
}
|
|
hal.console->println("Out of analog channels");
|
|
return NULL;
|
|
}
|
|
|
|
#endif // CONFIG_HAL_BOARD
|