/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see .
*
* Code by Andrew Tridgell and Siddharth Bharat Purohit
*/
#include
#include "ch.h"
#include "hal.h"
#if HAL_USE_ADC == TRUE
#include "AnalogIn.h"
#if HAL_WITH_IO_MCU
#include
extern AP_IOMCU iomcu;
#endif
#ifndef CHIBIOS_ADC_MAVLINK_DEBUG
// this allows the first 6 analog channels to be reported by mavlink for debugging purposes
#define CHIBIOS_ADC_MAVLINK_DEBUG 0
#endif
#include
#define ANLOGIN_DEBUGGING 0
// base voltage scaling for 12 bit 3.3V ADC
#define 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;
using namespace ChibiOS;
/*
scaling table between ADC count and actual input voltage, to account
for voltage dividers on the board.
*/
const AnalogIn::pin_info AnalogIn::pin_config[] = HAL_ANALOG_PINS;
#define ADC_GRP1_NUM_CHANNELS ARRAY_SIZE_SIMPLE(AnalogIn::pin_config)
// samples filled in by ADC DMA engine
adcsample_t AnalogIn::samples[ADC_DMA_BUF_DEPTH*ADC_GRP1_NUM_CHANNELS];
uint32_t AnalogIn::sample_sum[ADC_GRP1_NUM_CHANNELS];
uint32_t AnalogIn::sample_count;
AnalogSource::AnalogSource(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)
{
}
float AnalogSource::read_average()
{
if (_sum_count == 0) {
return _value;
}
_value = _sum_value / _sum_count;
_value_ratiometric = _sum_ratiometric / _sum_count;
_sum_value = 0;
_sum_ratiometric = 0;
_sum_count = 0;
return _value;
}
float AnalogSource::read_latest()
{
return _latest_value;
}
/*
return scaling from ADC count to Volts
*/
float AnalogSource::_pin_scaler(void)
{
float scaling = VOLTAGE_SCALING;
for (uint8_t i=0; i_pin) {
// add a value
c->_add_value(buf_adc[i], _board_voltage);
}
}
}
#if HAL_WITH_IO_MCU
// now handle special inputs from IOMCU
_servorail_voltage = iomcu.get_vservo();
#endif
#if CHIBIOS_ADC_MAVLINK_DEBUG
static uint8_t count;
if (AP_HAL::millis() > 5000 && count++ == 10) {
count = 0;
uint16_t adc[6] {};
for (uint8_t i=0; i < ADC_GRP1_NUM_CHANNELS; i++) {
adc[i] = buf_adc[i];
}
mavlink_msg_ap_adc_send(MAVLINK_COMM_0, adc[0], adc[1], adc[2], adc[3], adc[4], adc[5]);
}
#endif
}
AP_HAL::AnalogSource* AnalogIn::channel(int16_t pin)
{
for (uint8_t j=0; jprintf("Out of analog channels\n");
return nullptr;
}
/*
update power status flags
*/
void AnalogIn::update_power_flags(void)
{
uint16_t flags = 0;
#ifdef HAL_GPIO_PIN_VDD_BRICK_VALID
if (!palReadLine(HAL_GPIO_PIN_VDD_BRICK_VALID)) {
flags |= MAV_POWER_STATUS_BRICK_VALID;
}
#endif
#ifdef HAL_GPIO_PIN_VDD_SERVO_VALID
if (!palReadLine(HAL_GPIO_PIN_VDD_SERVO_VALID)) {
flags |= MAV_POWER_STATUS_SERVO_VALID;
}
#endif
#ifdef HAL_GPIO_PIN_VBUS
if (palReadLine(HAL_GPIO_PIN_VBUS)) {
flags |= MAV_POWER_STATUS_USB_CONNECTED;
}
#endif
#ifdef HAL_GPIO_PIN_VDD_5V_HIPOWER_OC
if (!palReadLine(HAL_GPIO_PIN_VDD_5V_HIPOWER_OC)) {
flags |= MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT;
}
#endif
#ifdef HAL_GPIO_PIN_VDD_5V_PERIPH_OC
if (!palReadLine(HAL_GPIO_PIN_VDD_5V_PERIPH_OC)) {
flags |= MAV_POWER_STATUS_PERIPH_OVERCURRENT;
}
#endif
if (_power_flags != 0 &&
_power_flags != flags &&
hal.util->get_soft_armed()) {
// the power status has changed while armed
flags |= MAV_POWER_STATUS_CHANGED;
}
_power_flags = flags;
}
#endif // HAL_USE_ADC