Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Oilpan ADC parameters added

Browse Source 
including a function that configures each Accel Axis
Defaults matching old behavior
Added param group
This commit is contained in:
Jason Short 2012-07-18 22:56:13 -07:00
parent e09d6ad7a8
commit c9d3ab5413
5 changed files with 157 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/Parameters.h
View File

@ -50,6 +50,7 @@ public:
//
k_param_format_version = 0,
k_param_software_type,
k_param_ins,
// simulation
k_param_sitl = 10,
@ -216,7 +217,7 @@ public:
AP_Int8 sonar_enabled;
AP_Int8 sonar_type; // 0 = XL, 1 = LV,
// 2 = XLL (XL with 10m range)
// 3 = HRLV
// 3 = HRLV
AP_Int8 battery_monitoring; // 0=disabled, 3=voltage only, 4=voltage and current
AP_Float volt_div_ratio;
AP_Float curr_amp_per_volt;

6
ArduCopter/Parameters.pde
View File

@ -57,7 +57,7 @@ static const AP_Param::Info var_info[] PROGMEM = {
// @Param: BATT_CAPACITY
// @DisplayName: Battery Capacity
// @Description: Battery capacity in milliamp-hours (mAh)
// @Units: mAh
// @Units: mAh
GSCALAR(pack_capacity, "BATT_CAPACITY"),
// @Param: MAG_ENABLE
@ -307,6 +307,10 @@ static const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/AP_Compass/Compass.cpp
GOBJECT(compass, "COMPASS_", Compass),
// @Group: INS_
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
GOBJECT(ins, "INS_", AP_InertialSensor_Oilpan),
GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
GOBJECT(gcs3, "SR3_", GCS_MAVLINK),

89
ArduCopter/setup.pde
View File

@ -6,6 +6,7 @@
static int8_t setup_radio (uint8_t argc, const Menu::arg *argv);
static int8_t setup_motors (uint8_t argc, const Menu::arg *argv);
static int8_t setup_accel (uint8_t argc, const Menu::arg *argv);
static int8_t setup_accel_scale (uint8_t argc, const Menu::arg *argv);
static int8_t setup_frame (uint8_t argc, const Menu::arg *argv);
static int8_t setup_factory (uint8_t argc, const Menu::arg *argv);
static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
@ -35,6 +36,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
{"frame", setup_frame},
{"motors", setup_motors},
{"level", setup_accel},
{"accel", setup_accel_scale},
{"modes", setup_flightmodes},
{"battery", setup_batt_monitor},
{"sonar", setup_sonar},
@ -252,6 +254,75 @@ setup_accel(uint8_t argc, const Menu::arg *argv)
return(0);
}
static int8_t
setup_accel_scale(uint8_t argc, const Menu::arg *argv)
{
#if CONFIG_ADC == ENABLED
int8_t accel_num;
float accel_avg = 0;
if (!strcmp_P(argv[1].str, PSTR("x"))) {
accel_num = 4;
}else if (!strcmp_P(argv[1].str, PSTR("y"))) {
accel_num = 5;
}else if (!strcmp_P(argv[1].str, PSTR("z"))) {
accel_num = 6;
}else{
Serial.printf_P(PSTR("x, y, or z\n"));
return 0;
}
print_hit_enter();
Serial.printf_P(PSTR("ADC\n"));
adc.Init(&timer_scheduler); // APM ADC library initialization
int16_t low, high;
delay(1000);
accel_avg = adc.Ch(accel_num);
low = high = accel_avg;
while(1){
delay(50);
accel_avg = accel_avg * .95 + adc.Ch(accel_num) * .05;
if(accel_avg > high)
high = ceil(accel_avg);
if(accel_avg < low)
low = floor(accel_avg);
Serial.printf_P(PSTR("%1.2f, %d, %d\n"), accel_avg, low, high);
if(Serial.available() > 0){
if(wait_for_yes()){
if(accel_num == 4){
ins._x_high = high;
ins._x_low = low;
ins._x_high.save();
ins._x_low.save();
}else if(accel_num == 5){
ins._y_high = high;
ins._y_low = low;
ins._y_high.save();
ins._y_low.save();
}else{
ins._z_high = high;
ins._z_low = low;
ins._z_high.save();
ins._z_low.save();
}
print_done();
}
return (0);
}
}
#else
return 0;
#endif // CONFIG_ADC
}
static int8_t
setup_frame(uint8_t argc, const Menu::arg *argv)
{
@ -1088,6 +1159,24 @@ print_blanks(int num)
}
}
static bool
wait_for_yes()
{
int c;
Serial.flush();
Serial.printf_P(PSTR("Y to save\n"));
do {
c = Serial.read();
} while (-1 == c);
if (('y' == c) || ('Y' == c))
return true;
else
return false;
}
static void
print_divider(void)
{

48
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
View File

@ -25,7 +25,9 @@ const float AP_InertialSensor_Oilpan::_adc_constraint = 900;
const float AP_InertialSensor_Oilpan::_gravity = 423.8;
///< would like to use _gravity here, but cannot
const float AP_InertialSensor_Oilpan::_accel_scale = 9.80665 / 423.8;
//const float AP_InertialSensor_Oilpan::_accel_x_scale = 9.80665 / 413.195;
//const float AP_InertialSensor_Oilpan::_accel_y_scale = 9.80665 / 412.985;
//const float AP_InertialSensor_Oilpan::_accel_z_scale = 9.80665 / 403.69;
#define ToRad(x) (x*0.01745329252) // *pi/180
// IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s,
@ -35,10 +37,28 @@ const float AP_InertialSensor_Oilpan::_gyro_gain_x = ToRad(0.4);
const float AP_InertialSensor_Oilpan::_gyro_gain_y = ToRad(0.41);
const float AP_InertialSensor_Oilpan::_gyro_gain_z = ToRad(0.41);
const AP_Param::GroupInfo AP_InertialSensor_Oilpan::var_info[] PROGMEM = {
// index 0 was used for the old orientation matrix
AP_GROUPINFO("XH", 0, AP_InertialSensor_Oilpan, _x_high),
AP_GROUPINFO("XL", 1, AP_InertialSensor_Oilpan, _x_low),
AP_GROUPINFO("YH", 2, AP_InertialSensor_Oilpan, _y_high),
AP_GROUPINFO("YL", 3, AP_InertialSensor_Oilpan, _y_low),
AP_GROUPINFO("ZH", 4, AP_InertialSensor_Oilpan, _z_high),
AP_GROUPINFO("ZL", 5, AP_InertialSensor_Oilpan, _z_low),
AP_GROUPEND
};
/* ------ Public functions -------------------------------------------*/
AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) :
_adc(adc)
_adc(adc),
_x_high(2465),
_x_low(1617),
_y_high(2465),
_y_low(1617),
_z_high(2465),
_z_low(1617)
{
_gyro.x = 0;
_gyro.y = 0;
@ -52,6 +72,13 @@ uint16_t AP_InertialSensor_Oilpan::init( AP_PeriodicProcess * scheduler)
{
_adc->Init(scheduler);
_accel_mid.x = (_x_high + _x_low) / 2;
_accel_mid.y = (_y_high + _y_low) / 2;
_accel_mid.z = (_z_high + _z_low) / 2;
_accel_scale.x = 9.80665 / ((float)_x_high - _accel_mid.x);
_accel_scale.y = 9.80665 / ((float)_y_high - _accel_mid.y);
_accel_scale.z = 9.80665 / ((float)_z_high - _accel_mid.z);
#if defined(DESKTOP_BUILD)
return AP_PRODUCT_ID_SITL;
#elif defined(__AVR_ATmega1280__)
@ -72,9 +99,20 @@ bool AP_InertialSensor_Oilpan::update()
_gyro.y = _gyro_gain_y * _sensor_signs[1] * _gyro_apply_std_offset( adc_values[1] );
_gyro.z = _gyro_gain_z * _sensor_signs[2] * _gyro_apply_std_offset( adc_values[2] );
_accel.x = _accel_scale * _sensor_signs[3] * _accel_apply_std_offset( adc_values[3] );
_accel.y = _accel_scale * _sensor_signs[4] * _accel_apply_std_offset( adc_values[4] );
_accel.z = _accel_scale * _sensor_signs[5] * _accel_apply_std_offset( adc_values[5] );
// _accel.x = _accel_x_scale * _sensor_signs[3] * _accel_apply_std_offset( adc_values[3] );
// _accel.y = _accel_y_scale * _sensor_signs[4] * _accel_apply_std_offset( adc_values[4] );
// _accel.z = _accel_z_scale * _sensor_signs[5] * _accel_apply_std_offset( adc_values[5] );
_accel.x = _accel_scale.x * _sensor_signs[3] * (adc_values[3] - _accel_mid.x);
_accel.y = _accel_scale.y * _sensor_signs[4] * (adc_values[4] - _accel_mid.y);
_accel.z = _accel_scale.z * _sensor_signs[5] * (adc_values[5] - _accel_mid.z);
/*
X = 1619.30 to 2445.69
Y = 1609.45 to 2435.42
Z = 1627.44 to 2434.82
*/
return true;
}

21
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h
View File

@ -34,11 +34,27 @@ class AP_InertialSensor_Oilpan : public AP_InertialSensor
void reset_sample_time();
float get_gyro_drift_rate();
private:
static const struct AP_Param::GroupInfo var_info[];
AP_ADC *_adc;
AP_Int16 _x_high;
AP_Int16 _x_low;
AP_Int16 _y_high;
AP_Int16 _y_low;
AP_Int16 _z_high;
AP_Int16 _z_low;
Vector3f _accel_scale;
private:
Vector3f _gyro;
Vector3f _accel;
Vector3f _accel_high;
Vector3f _accel_low;
Vector3f _accel_mid;
AP_ADC *_adc;
float _temp;
uint32_t _sample_time;
@ -48,7 +64,6 @@ class AP_InertialSensor_Oilpan : public AP_InertialSensor
static const uint8_t _gyro_temp_ch;
static const float _gravity;
static const float _accel_scale;
static const float _gyro_gain_x;
static const float _gyro_gain_y;