diff --git a/libraries/AP_ADC/AP_ADC.h b/libraries/AP_ADC/AP_ADC.h index 6e8f6c75c9..f9c0ce95da 100644 --- a/libraries/AP_ADC/AP_ADC.h +++ b/libraries/AP_ADC/AP_ADC.h @@ -58,6 +58,5 @@ private: #include "AP_ADC_ADS7844.h" #include "AP_ADC_ADS1115.h" -#include "AP_ADC_HIL.h" #endif diff --git a/libraries/AP_ADC/AP_ADC_HIL.cpp b/libraries/AP_ADC/AP_ADC_HIL.cpp deleted file mode 100644 index 1d0568465a..0000000000 --- a/libraries/AP_ADC/AP_ADC_HIL.cpp +++ /dev/null @@ -1,85 +0,0 @@ - -#include "AP_ADC_HIL.h" -#include -extern const AP_HAL::HAL& hal; -/* - 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 . -*/ -/* - * AP_ADC_HIL.cpp - * Author: James Goppert - * - */ - -const uint8_t AP_ADC_HIL::sensors[6] = {1,2,0,4,5,6}; -const int8_t AP_ADC_HIL::sensorSign[6] = { 1, -1, -1,-1, 1, 1}; -const float AP_ADC_HIL::gyroBias[3] = {1665,1665,1665}; -const float AP_ADC_HIL::accelBias[3] = {2025,2025,2025}; -// gyroScale = 1/[GyroGain*pi/180] GyroGains (0.4,0.41,0.41) -const float AP_ADC_HIL::gyroScale[3] = {143.239f, 139.746f, 139.746f}; -const float AP_ADC_HIL::accelScale[3] = {418,418,418}; // adcPerG - -AP_ADC_HIL::AP_ADC_HIL() -{ - // gyros set to zero for calibration - setGyro(0,0); - setGyro(1,0); - setGyro(2,0); - - // accels set to zero for calibration - setAccel(0,0); - setAccel(1,0); - setAccel(2,0); - - // set diff press and temp to zero - setGyroTemp(0); - setPressure(0); -} - -void AP_ADC_HIL::Init() -{ - hal.scheduler->register_timer_process( AP_HAL_MEMBERPROC(&AP_ADC_HIL::read)); -} - -// Read one channel value -float AP_ADC_HIL::Ch(unsigned char ch_num) -{ - return adcValue[ch_num]; -} - -// Read 6 channel values -uint32_t AP_ADC_HIL::Ch6(const uint8_t *channel_numbers, float *result) -{ - _count = 0; - - for (uint8_t i=0; i<6; i++) { - result[i] = Ch(channel_numbers[i]); - } - uint32_t now = hal.scheduler->micros(); - uint32_t ret = now - _last_ch6_time; - _last_ch6_time = now; - return ret; -} - -// see if new data is available -bool AP_ADC_HIL::new_data_available(const uint8_t *channel_numbers) -{ - return true; -} - -// Get minimum number of samples read from the sensors -uint16_t AP_ADC_HIL::num_samples_available(const uint8_t *channel_numbers) -{ - return _count; -} diff --git a/libraries/AP_ADC/AP_ADC_HIL.h b/libraries/AP_ADC/AP_ADC_HIL.h deleted file mode 100644 index c39b00c268..0000000000 --- a/libraries/AP_ADC/AP_ADC_HIL.h +++ /dev/null @@ -1,114 +0,0 @@ -#ifndef AP_ADC_HIL_H -#define AP_ADC_HIL_H -/* - 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 . - */ - -/* - * AP_ADC_HIL.h - * Author: James Goppert - * - */ - -#include -#include "AP_ADC.h" - -/// -// A hardware in the loop model of the ADS7844 analog to digital converter -// @author James Goppert DIYDrones.com -class AP_ADC_HIL : public AP_ADC -{ -public: - - /// - // Constructor - AP_ADC_HIL(); // Constructor - - /// - // Initializes sensor, part of public AP_ADC interface - void Init(); - - /// - // Read the sensor, part of public AP_ADC interface - float Ch(unsigned char ch_num); - - /// - // Read 6 sensors at once - uint32_t Ch6(const uint8_t *channel_numbers, float *result); - - // see if Ch6 would block - bool new_data_available(const uint8_t *channel_numbers); - - // Get minimum number of samples read from the sensors - uint16_t num_samples_available(const uint8_t *channel_numbers); - -private: - - /// - // The raw adc array - uint16_t adcValue[8]; - - // the time in microseconds when Ch6 was last requested - uint32_t _last_ch6_time; - - /// - // sensor constants - // constants declared in cpp file - // @see AP_ADC_HIL.cpp - static const uint8_t sensors[6]; - static const float gyroBias[3]; - static const float gyroScale[3]; - static const float accelBias[3]; - static const float accelScale[3]; - static const int8_t sensorSign[6]; - - /// - // gyro set function - // @param val the value of the gyro in milli rad/s - // @param index the axis for the gyro(0-x,1-y,2-z) - inline void setGyro(uint8_t index, int16_t val) { - int16_t temp = val * gyroScale[index] / 1000 + gyroBias[index]; - adcValue[sensors[index]] = (sensorSign[index] < 0) ? -temp : temp; - } - - /// - // accel set function - // @param val the value of the accel in milli g's - // @param index the axis for the accelerometer(0-x,1-y,2-z) - inline void setAccel(uint8_t index, int16_t val) { - int16_t temp = val * accelScale[index] / 1000 + accelBias[index]; - adcValue[sensors[index+3]] = (sensorSign[index+3] < 0) ? -temp : temp; - } - - /// - // Sets the differential pressure adc channel - // TODO: implement - void setPressure(int16_t val) { - } - - /// - // Sets the gyro temp adc channel - // TODO: implement - void setGyroTemp(int16_t val) { - } - - // read function that pretends to capture new data - void read(void) { - _count++; - } - - uint16_t _count; // number of samples captured -}; - -#endif