mirror of https://github.com/ArduPilot/ardupilot
162 lines
4.5 KiB
C++
162 lines
4.5 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
/*
|
|
This program 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 program 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 <http://www.gnu.org/licenses/>.
|
|
*/
|
|
/*
|
|
|
|
(c) 2017 night_ghost@ykoctpa.ru
|
|
|
|
based on: Flymaple port by Mike McCauley
|
|
*/
|
|
|
|
#pragma GCC optimize ("O2")
|
|
|
|
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT
|
|
|
|
#include "AP_HAL_F4Light_Namespace.h"
|
|
#include "AnalogIn.h"
|
|
#include <adc.h>
|
|
#include <boards.h>
|
|
#include <gpio_hal.h>
|
|
#include "GPIO.h"
|
|
#include "Scheduler.h"
|
|
#include "c++.h"
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
using namespace F4Light;
|
|
|
|
/* CHANNEL_READ_REPEAT: how many reads on a channel before using the value.
|
|
* This seems to be determined empirically */
|
|
#define CHANNEL_READ_REPEAT 1
|
|
|
|
|
|
|
|
|
|
AnalogSource* IN_CCM AnalogIn::_channels[F4Light_INPUT_MAX_CHANNELS];
|
|
AnalogSource IN_CCM AnalogIn::_vcc(ANALOG_INPUT_F4Light_VCC);
|
|
|
|
|
|
AnalogIn::AnalogIn(){}
|
|
|
|
|
|
void AnalogIn::init() {
|
|
|
|
// Register _timer_event in the scheduler.
|
|
void *_task = Scheduler::start_task(FUNCTOR_BIND_MEMBER(&AnalogIn::_timer_event, void), 256); // small stack
|
|
if(_task){
|
|
Scheduler::set_task_priority(_task, DRIVER_PRIORITY+1); // slightly less
|
|
Scheduler::set_task_period(_task, 2000); // setting of period allows task to run
|
|
}
|
|
|
|
_register_channel(&_vcc); // Register each private channel with AnalogIn.
|
|
|
|
cnv_started=false;
|
|
}
|
|
|
|
|
|
AnalogSource* AnalogIn::_create_channel(uint8_t chnum) {
|
|
|
|
#if 0
|
|
AnalogSource *ch = new AnalogSource(chnum);
|
|
#else
|
|
caddr_t ptr = sbrk_ccm(sizeof(AnalogSource)); // allocate memory in CCM
|
|
AnalogSource *ch = new(ptr) AnalogSource(chnum);
|
|
#endif
|
|
|
|
_register_channel(ch);
|
|
return ch;
|
|
}
|
|
|
|
void AnalogIn::_register_channel(AnalogSource* ch) {
|
|
if (_num_channels >= F4Light_INPUT_MAX_CHANNELS) {
|
|
AP_HAL::panic("Error: AP_HAL_F4Light::AnalogIn out of channels\r\n");
|
|
}
|
|
_channels[_num_channels] = ch;
|
|
|
|
// *NO* need to lock to increment _num_channels INSPITE OF it is used by the interrupt to access _channels
|
|
// because we first fill _channels[]
|
|
_num_channels++;
|
|
}
|
|
|
|
|
|
void AnalogIn::_timer_event(void)
|
|
{
|
|
|
|
|
|
if (_num_channels == 0) return; /* No channels are registered - nothing to be done. */
|
|
|
|
const adc_dev *dev = _channels[_active_channel]->_find_device();
|
|
|
|
if (_channels[_active_channel]->_pin == ANALOG_INPUT_NONE || !_channels[_active_channel]->initialized()) {
|
|
_channels[_active_channel]->new_sample(0);
|
|
goto next_channel;
|
|
}
|
|
|
|
if (cnv_started && !(dev->adcx->SR & ADC_SR_EOC)) {
|
|
// ADC Conversion is still running - this should not happens, as we are called at 1khz.
|
|
// SO - more likely we forget to start conversion or some went wrong...
|
|
// let's fix it
|
|
adc_start_conv(dev);
|
|
return;
|
|
}
|
|
|
|
_channel_repeat_count++;
|
|
if (_channel_repeat_count < CHANNEL_READ_REPEAT ||
|
|
!_channels[_active_channel]->reading_settled()) {
|
|
// Start a new conversion on the same channel, throw away the current conversion
|
|
adc_start_conv(dev);
|
|
|
|
cnv_started=true;
|
|
return;
|
|
}
|
|
|
|
_channel_repeat_count = 0;
|
|
if (cnv_started) {
|
|
uint16_t sample = (uint16_t)(dev->adcx->DR & ADC_DR_DATA);
|
|
/* Give the active channel a new sample */
|
|
_channels[_active_channel]->new_sample( sample );
|
|
}
|
|
|
|
next_channel:
|
|
|
|
_channels[_active_channel]->stop_read(); /* stop the previous channel, if a stop pin is defined */
|
|
_active_channel = (_active_channel + 1) % _num_channels; /* Move to the next channel */
|
|
_channels[_active_channel]->setup_read(); /* Setup the next channel's conversion */
|
|
dev = _channels[_active_channel]->_find_device();
|
|
|
|
if(dev != NULL) {
|
|
/* Start conversion */
|
|
adc_start_conv(dev);
|
|
cnv_started=true;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
AP_HAL::AnalogSource* AnalogIn::channel(int16_t ch)
|
|
{
|
|
if ((uint8_t)ch == ANALOG_INPUT_F4Light_VCC) {
|
|
return &_vcc;
|
|
} else {
|
|
return _create_channel((uint8_t)ch);
|
|
}
|
|
}
|
|
|
|
#endif
|