Oilpan ADC parameters added

including a function that configures each Accel Axis
Defaults matching old behavior
Added param group

ArduCopter/Parameters.h

@@ -50,6 +50,7 @@ public:
 	//
 	k_param_format_version = 0,
 	k_param_software_type,
+	k_param_ins,
 
     // simulation
     k_param_sitl = 10,

ArduCopter/Parameters.pde

@@ -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),
 

ArduCopter/setup.pde

@@ -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)
 {

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp

@@ -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;
 }

libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h

@@ -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;