aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

uncrustify libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp

Browse Source
This commit is contained in:
uncrustify 2012-08-16 23:19:56 -07:00 committed by Pat Hickey
parent a946ed762b
commit bc6d3811bc
1 changed files with 84 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

151
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
View File

@ -7,8 +7,8 @@
const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 }; const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 };
// ADC result sign adjustment for sensors. // ADC result sign adjustment for sensors.
const int8_t AP_InertialSensor_Oilpan::_sensor_signs[6] = const int8_t AP_InertialSensor_Oilpan::_sensor_signs[6] =
{ 1, -1, -1, 1, -1 , -1 }; { 1, -1, -1, 1, -1, -1 };
// ADC channel reading the gyro temperature // ADC channel reading the gyro temperature
const uint8_t AP_InertialSensor_Oilpan::_gyro_temp_ch = 3; const uint8_t AP_InertialSensor_Oilpan::_gyro_temp_ch = 3;
@ -39,12 +39,12 @@ const float AP_InertialSensor_Oilpan::_gyro_gain_z = ToRad(0.41);
const AP_Param::GroupInfo AP_InertialSensor_Oilpan::var_info[] PROGMEM = { const AP_Param::GroupInfo AP_InertialSensor_Oilpan::var_info[] PROGMEM = {
// index 0 was used for the old orientation matrix // index 0 was used for the old orientation matrix
AP_GROUPINFO("XH", 0, AP_InertialSensor_Oilpan, _x_high, 2465), AP_GROUPINFO("XH", 0, AP_InertialSensor_Oilpan, _x_high, 2465),
AP_GROUPINFO("XL", 1, AP_InertialSensor_Oilpan, _x_low, 1617), AP_GROUPINFO("XL", 1, AP_InertialSensor_Oilpan, _x_low, 1617),
AP_GROUPINFO("YH", 2, AP_InertialSensor_Oilpan, _y_high, 2465), AP_GROUPINFO("YH", 2, AP_InertialSensor_Oilpan, _y_high, 2465),
AP_GROUPINFO("YL", 3, AP_InertialSensor_Oilpan, _y_low, 1617), AP_GROUPINFO("YL", 3, AP_InertialSensor_Oilpan, _y_low, 1617),
AP_GROUPINFO("ZH", 4, AP_InertialSensor_Oilpan, _z_high, 2465), AP_GROUPINFO("ZH", 4, AP_InertialSensor_Oilpan, _z_high, 2465),
AP_GROUPINFO("ZL", 5, AP_InertialSensor_Oilpan, _z_low, 1617), AP_GROUPINFO("ZL", 5, AP_InertialSensor_Oilpan, _z_low, 1617),
AP_GROUPEND AP_GROUPEND
}; };
@ -52,63 +52,63 @@ const AP_Param::GroupInfo AP_InertialSensor_Oilpan::var_info[] PROGMEM = {
/* ------ Public functions -------------------------------------------*/ /* ------ Public functions -------------------------------------------*/
AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) : AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) :
_adc(adc) _adc(adc)
{ {
_gyro.x = 0; _gyro.x = 0;
_gyro.y = 0; _gyro.y = 0;
_gyro.z = 0; _gyro.z = 0;
_accel.x = 0; _accel.x = 0;
_accel.y = 0; _accel.y = 0;
_accel.z = 0; _accel.z = 0;
} }
uint16_t AP_InertialSensor_Oilpan::init( AP_PeriodicProcess * scheduler) uint16_t AP_InertialSensor_Oilpan::init( AP_PeriodicProcess * scheduler)
{ {
_adc->Init(scheduler); _adc->Init(scheduler);
_accel_mid.x = (_x_high + _x_low) / 2; _accel_mid.x = (_x_high + _x_low) / 2;
_accel_mid.y = (_y_high + _y_low) / 2; _accel_mid.y = (_y_high + _y_low) / 2;
_accel_mid.z = (_z_high + _z_low) / 2; _accel_mid.z = (_z_high + _z_low) / 2;
_accel_scale.x = 9.80665 / ((float)_x_high - _accel_mid.x); _accel_scale.x = 9.80665 / ((float)_x_high - _accel_mid.x);
_accel_scale.y = 9.80665 / ((float)_y_high - _accel_mid.y); _accel_scale.y = 9.80665 / ((float)_y_high - _accel_mid.y);
_accel_scale.z = 9.80665 / ((float)_z_high - _accel_mid.z); _accel_scale.z = 9.80665 / ((float)_z_high - _accel_mid.z);
#if defined(DESKTOP_BUILD) #if defined(DESKTOP_BUILD)
return AP_PRODUCT_ID_SITL; return AP_PRODUCT_ID_SITL;
#elif defined(__AVR_ATmega1280__) #elif defined(__AVR_ATmega1280__)
return AP_PRODUCT_ID_APM1_1280; return AP_PRODUCT_ID_APM1_1280;
#else #else
return AP_PRODUCT_ID_APM1_2560; return AP_PRODUCT_ID_APM1_2560;
#endif #endif
} }
bool AP_InertialSensor_Oilpan::update() bool AP_InertialSensor_Oilpan::update()
{ {
float adc_values[6]; float adc_values[6];
_sample_time = _adc->Ch6(_sensors, adc_values); _sample_time = _adc->Ch6(_sensors, adc_values);
_temp = _adc->Ch(_gyro_temp_ch); _temp = _adc->Ch(_gyro_temp_ch);
_gyro.x = _gyro_gain_x * _sensor_signs[0] * _gyro_apply_std_offset( adc_values[0] ); _gyro.x = _gyro_gain_x * _sensor_signs[0] * _gyro_apply_std_offset( adc_values[0] );
_gyro.y = _gyro_gain_y * _sensor_signs[1] * _gyro_apply_std_offset( adc_values[1] ); _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] ); _gyro.z = _gyro_gain_z * _sensor_signs[2] * _gyro_apply_std_offset( adc_values[2] );
// _accel.x = _accel_x_scale * _sensor_signs[3] * _accel_apply_std_offset( adc_values[3] ); // _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.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.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.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.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); _accel.z = _accel_scale.z * _sensor_signs[5] * (adc_values[5] - _accel_mid.z);
/* /*
X = 1619.30 to 2445.69 * X = 1619.30 to 2445.69
Y = 1609.45 to 2435.42 * Y = 1609.45 to 2435.42
Z = 1627.44 to 2434.82 * Z = 1627.44 to 2434.82
*/ */
return true; return true;
} }
bool AP_InertialSensor_Oilpan::new_data_available( void ) bool AP_InertialSensor_Oilpan::new_data_available( void )
@ -116,55 +116,72 @@ bool AP_InertialSensor_Oilpan::new_data_available( void )
return _adc->new_data_available(_sensors); return _adc->new_data_available(_sensors);
} }
float AP_InertialSensor_Oilpan::gx() { return _gyro.x; } float AP_InertialSensor_Oilpan::gx() {
float AP_InertialSensor_Oilpan::gy() { return _gyro.y; } return _gyro.x;
float AP_InertialSensor_Oilpan::gz() { return _gyro.z; } }
float AP_InertialSensor_Oilpan::gy() {
return _gyro.y;
}
float AP_InertialSensor_Oilpan::gz() {
return _gyro.z;
}
void AP_InertialSensor_Oilpan::get_gyros( float * g ) void AP_InertialSensor_Oilpan::get_gyros( float * g )
{ {
g[0] = _gyro.x; g[0] = _gyro.x;
g[1] = _gyro.y; g[1] = _gyro.y;
g[2] = _gyro.z; g[2] = _gyro.z;
} }
float AP_InertialSensor_Oilpan::ax() { return _accel.x; } float AP_InertialSensor_Oilpan::ax() {
float AP_InertialSensor_Oilpan::ay() { return _accel.y; } return _accel.x;
float AP_InertialSensor_Oilpan::az() { return _accel.z; } }
float AP_InertialSensor_Oilpan::ay() {
return _accel.y;
}
float AP_InertialSensor_Oilpan::az() {
return _accel.z;
}
void AP_InertialSensor_Oilpan::get_accels( float * a ) void AP_InertialSensor_Oilpan::get_accels( float * a )
{ {
a[0] = _accel.x; a[0] = _accel.x;
a[1] = _accel.y; a[1] = _accel.y;
a[2] = _accel.z; a[2] = _accel.z;
} }
void AP_InertialSensor_Oilpan::get_sensors( float * sensors ) void AP_InertialSensor_Oilpan::get_sensors( float * sensors )
{ {
sensors[0] = _gyro.x; sensors[0] = _gyro.x;
sensors[1] = _gyro.y; sensors[1] = _gyro.y;
sensors[2] = _gyro.z; sensors[2] = _gyro.z;
sensors[3] = _accel.x; sensors[3] = _accel.x;
sensors[4] = _accel.y; sensors[4] = _accel.y;
sensors[5] = _accel.z; sensors[5] = _accel.z;
} }
float AP_InertialSensor_Oilpan::temperature() { return _temp; } float AP_InertialSensor_Oilpan::temperature() {
return _temp;
}
uint32_t AP_InertialSensor_Oilpan::sample_time() { return _sample_time; } uint32_t AP_InertialSensor_Oilpan::sample_time() {
void AP_InertialSensor_Oilpan::reset_sample_time() { } return _sample_time;
}
void AP_InertialSensor_Oilpan::reset_sample_time() {
}
/* ------ Private functions -------------------------------------------*/ /* ------ Private functions -------------------------------------------*/
float AP_InertialSensor_Oilpan::_gyro_apply_std_offset( float adc_value ) float AP_InertialSensor_Oilpan::_gyro_apply_std_offset( float adc_value )
{ {
/* Magic number from AP_ADC_Oilpan.h */ /* Magic number from AP_ADC_Oilpan.h */
return ((float) adc_value ) - 1658.0f; return ((float) adc_value ) - 1658.0f;
} }
float AP_InertialSensor_Oilpan::_accel_apply_std_offset( float adc_value ) float AP_InertialSensor_Oilpan::_accel_apply_std_offset( float adc_value )
{ {
/* Magic number from AP_ADC_Oilpan.h */ /* Magic number from AP_ADC_Oilpan.h */
return ((float) adc_value ) - 2041.0f; return ((float) adc_value ) - 2041.0f;
} }
// return the oilpan gyro drift rate in radian/s/s // return the oilpan gyro drift rate in radian/s/s