You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
100 lines
3.0 KiB
100 lines
3.0 KiB
/* |
|
www.ArduCopter.com - www.DIYDrones.com |
|
Copyright (c) 2010. All rights reserved. |
|
An Open Source Arduino based multicopter. |
|
|
|
File : Sensors.pde |
|
Version : v1.0, Aug 27, 2010 |
|
Author(s): ArduCopter Team |
|
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, |
|
Jani Hirvinen, Ken McEwans, Roberto Navoni, |
|
Sandro Benigno, Chris Anderson |
|
|
|
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/>. |
|
|
|
* ************************************************************** */ |
|
|
|
/* ******* ADC functions ********************* */ |
|
// Read all the ADC channles |
|
void Read_adc_raw(void) |
|
{ |
|
//int temp; |
|
|
|
for (int i=0;i<6;i++) |
|
AN[i] = APM_ADC.Ch(sensors[i]); |
|
} |
|
|
|
// Returns an analog value with the offset |
|
int read_adc(int select) |
|
{ |
|
if (SENSOR_SIGN[select]<0) |
|
return (AN_OFFSET[select]-AN[select]); |
|
else |
|
return (AN[select]-AN_OFFSET[select]); |
|
} |
|
|
|
void calibrateSensors(void) { |
|
int i; |
|
int j = 0; |
|
byte gyro; |
|
float aux_float[3]; |
|
|
|
Read_adc_raw(); // Read sensors data |
|
delay(5); |
|
|
|
// Offset values for accels and gyros... |
|
AN_OFFSET[3] = acc_offset_x; // Accel offset values are taken from external calibration (In Configurator) |
|
AN_OFFSET[4] = acc_offset_y; |
|
AN_OFFSET[5] = acc_offset_z; |
|
aux_float[0] = gyro_offset_roll; |
|
aux_float[1] = gyro_offset_pitch; |
|
aux_float[2] = gyro_offset_yaw; |
|
|
|
// Take the gyro offset values |
|
for(i=0;i<600;i++) |
|
{ |
|
Read_adc_raw(); // Read sensors |
|
for(gyro=GYROZ; gyro<=GYROY; gyro++) |
|
aux_float[gyro]=aux_float[gyro]*0.8 + AN[gyro]*0.2; // Filtering |
|
Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],0); |
|
delay(5); |
|
RunningLights(j); // (in Functions.pde) |
|
// Runnings lights effect to let user know that we are taking mesurements |
|
if((i % 5) == 0) j++; |
|
if(j >= 3) j = 0; |
|
} |
|
|
|
// Switch off all ABC lights |
|
LightsOff(); |
|
|
|
for(gyro=GYROZ; gyro<=GYROY; gyro++) |
|
AN_OFFSET[gyro]=aux_float[gyro]; // Update sensor OFFSETs from values read |
|
} |
|
|
|
#ifdef UseBMP |
|
void read_baro(void) |
|
{ |
|
float tempPresAlt; |
|
|
|
tempPresAlt = float(APM_BMP085.Press)/101325.0; |
|
//tempPresAlt = pow(tempPresAlt, 0.190284); |
|
//press_alt = (1.0 - tempPresAlt) * 145366.45; |
|
tempPresAlt = pow(tempPresAlt, 0.190295); |
|
if (press_alt==0) |
|
press_alt = (1.0 - tempPresAlt) * 4433000; // Altitude in cm |
|
else |
|
press_alt = press_alt*0.9 + ((1.0 - tempPresAlt) * 443300); // Altitude in cm (filtered) |
|
} |
|
#endif |
|
|
|
|