zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

uncrustify libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp

master
uncrustify 13 years ago committed by Pat Hickey
parent
0c322fb18a
commit
77729b847f
1 changed files with 84 additions and 67 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 151
      libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp

151
libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
Unescape View File

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

Write Preview
Loading…
Cancel
Save