forked from Archive/PX4-Autopilot
sensors: Support for up to three sensors of the IMU types
This commit is contained in:
parent
c467d1635e
commit
b6bac2c88d
|
@ -199,9 +199,15 @@ private:
|
||||||
bool _hil_enabled; /**< if true, HIL is active */
|
bool _hil_enabled; /**< if true, HIL is active */
|
||||||
bool _publishing; /**< if true, we are publishing sensor data */
|
bool _publishing; /**< if true, we are publishing sensor data */
|
||||||
|
|
||||||
int _gyro_sub; /**< raw gyro data subscription */
|
int _gyro_sub; /**< raw gyro0 data subscription */
|
||||||
int _accel_sub; /**< raw accel data subscription */
|
int _accel_sub; /**< raw accel0 data subscription */
|
||||||
int _mag_sub; /**< raw mag data subscription */
|
int _mag_sub; /**< raw mag0 data subscription */
|
||||||
|
int _gyro1_sub; /**< raw gyro1 data subscription */
|
||||||
|
int _accel1_sub; /**< raw accel1 data subscription */
|
||||||
|
int _mag1_sub; /**< raw mag1 data subscription */
|
||||||
|
int _gyro2_sub; /**< raw gyro2 data subscription */
|
||||||
|
int _accel2_sub; /**< raw accel2 data subscription */
|
||||||
|
int _mag2_sub; /**< raw mag2 data subscription */
|
||||||
int _rc_sub; /**< raw rc channels data subscription */
|
int _rc_sub; /**< raw rc channels data subscription */
|
||||||
int _baro_sub; /**< raw baro data subscription */
|
int _baro_sub; /**< raw baro data subscription */
|
||||||
int _airspeed_sub; /**< airspeed subscription */
|
int _airspeed_sub; /**< airspeed subscription */
|
||||||
|
@ -478,6 +484,12 @@ Sensors::Sensors() :
|
||||||
_gyro_sub(-1),
|
_gyro_sub(-1),
|
||||||
_accel_sub(-1),
|
_accel_sub(-1),
|
||||||
_mag_sub(-1),
|
_mag_sub(-1),
|
||||||
|
_gyro1_sub(-1),
|
||||||
|
_accel1_sub(-1),
|
||||||
|
_mag1_sub(-1),
|
||||||
|
_gyro2_sub(-1),
|
||||||
|
_accel2_sub(-1),
|
||||||
|
_mag2_sub(-1),
|
||||||
_rc_sub(-1),
|
_rc_sub(-1),
|
||||||
_baro_sub(-1),
|
_baro_sub(-1),
|
||||||
_vcontrol_mode_sub(-1),
|
_vcontrol_mode_sub(-1),
|
||||||
|
@ -1019,6 +1031,48 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
|
||||||
|
|
||||||
raw.accelerometer_timestamp = accel_report.timestamp;
|
raw.accelerometer_timestamp = accel_report.timestamp;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
orb_check(_accel1_sub, &accel_updated);
|
||||||
|
|
||||||
|
if (accel_updated) {
|
||||||
|
struct accel_report accel_report;
|
||||||
|
|
||||||
|
orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report);
|
||||||
|
|
||||||
|
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
|
||||||
|
vect = _board_rotation * vect;
|
||||||
|
|
||||||
|
raw.accelerometer1_m_s2[0] = vect(0);
|
||||||
|
raw.accelerometer1_m_s2[1] = vect(1);
|
||||||
|
raw.accelerometer1_m_s2[2] = vect(2);
|
||||||
|
|
||||||
|
raw.accelerometer1_raw[0] = accel_report.x_raw;
|
||||||
|
raw.accelerometer1_raw[1] = accel_report.y_raw;
|
||||||
|
raw.accelerometer1_raw[2] = accel_report.z_raw;
|
||||||
|
|
||||||
|
raw.accelerometer1_timestamp = accel_report.timestamp;
|
||||||
|
}
|
||||||
|
|
||||||
|
orb_check(_accel2_sub, &accel_updated);
|
||||||
|
|
||||||
|
if (accel_updated) {
|
||||||
|
struct accel_report accel_report;
|
||||||
|
|
||||||
|
orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report);
|
||||||
|
|
||||||
|
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
|
||||||
|
vect = _board_rotation * vect;
|
||||||
|
|
||||||
|
raw.accelerometer2_m_s2[0] = vect(0);
|
||||||
|
raw.accelerometer2_m_s2[1] = vect(1);
|
||||||
|
raw.accelerometer2_m_s2[2] = vect(2);
|
||||||
|
|
||||||
|
raw.accelerometer2_raw[0] = accel_report.x_raw;
|
||||||
|
raw.accelerometer2_raw[1] = accel_report.y_raw;
|
||||||
|
raw.accelerometer2_raw[2] = accel_report.z_raw;
|
||||||
|
|
||||||
|
raw.accelerometer2_timestamp = accel_report.timestamp;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -1045,6 +1099,48 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
|
||||||
|
|
||||||
raw.timestamp = gyro_report.timestamp;
|
raw.timestamp = gyro_report.timestamp;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
orb_check(_gyro1_sub, &gyro_updated);
|
||||||
|
|
||||||
|
if (gyro_updated) {
|
||||||
|
struct gyro_report gyro_report;
|
||||||
|
|
||||||
|
orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report);
|
||||||
|
|
||||||
|
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
|
||||||
|
vect = _board_rotation * vect;
|
||||||
|
|
||||||
|
raw.gyro1_rad_s[0] = vect(0);
|
||||||
|
raw.gyro1_rad_s[1] = vect(1);
|
||||||
|
raw.gyro1_rad_s[2] = vect(2);
|
||||||
|
|
||||||
|
raw.gyro1_raw[0] = gyro_report.x_raw;
|
||||||
|
raw.gyro1_raw[1] = gyro_report.y_raw;
|
||||||
|
raw.gyro1_raw[2] = gyro_report.z_raw;
|
||||||
|
|
||||||
|
raw.gyro1_timestamp = gyro_report.timestamp;
|
||||||
|
}
|
||||||
|
|
||||||
|
orb_check(_gyro2_sub, &gyro_updated);
|
||||||
|
|
||||||
|
if (gyro_updated) {
|
||||||
|
struct gyro_report gyro_report;
|
||||||
|
|
||||||
|
orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report);
|
||||||
|
|
||||||
|
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
|
||||||
|
vect = _board_rotation * vect;
|
||||||
|
|
||||||
|
raw.gyro2_rad_s[0] = vect(0);
|
||||||
|
raw.gyro2_rad_s[1] = vect(1);
|
||||||
|
raw.gyro2_rad_s[2] = vect(2);
|
||||||
|
|
||||||
|
raw.gyro2_raw[0] = gyro_report.x_raw;
|
||||||
|
raw.gyro2_raw[1] = gyro_report.y_raw;
|
||||||
|
raw.gyro2_raw[2] = gyro_report.z_raw;
|
||||||
|
|
||||||
|
raw.gyro2_timestamp = gyro_report.timestamp;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -1060,6 +1156,8 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
|
||||||
|
|
||||||
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
|
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
|
||||||
|
|
||||||
|
// XXX we need device-id based handling here
|
||||||
|
|
||||||
if (_mag_is_external) {
|
if (_mag_is_external) {
|
||||||
vect = _external_mag_rotation * vect;
|
vect = _external_mag_rotation * vect;
|
||||||
|
|
||||||
|
@ -1621,6 +1719,12 @@ Sensors::task_main()
|
||||||
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0));
|
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0));
|
||||||
_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
|
_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
|
||||||
_mag_sub = orb_subscribe(ORB_ID(sensor_mag0));
|
_mag_sub = orb_subscribe(ORB_ID(sensor_mag0));
|
||||||
|
_gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
|
||||||
|
_accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
|
||||||
|
_mag1_sub = orb_subscribe(ORB_ID(sensor_mag1));
|
||||||
|
_gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2));
|
||||||
|
_accel2_sub = orb_subscribe(ORB_ID(sensor_accel2));
|
||||||
|
_mag2_sub = orb_subscribe(ORB_ID(sensor_mag2));
|
||||||
_rc_sub = orb_subscribe(ORB_ID(input_rc));
|
_rc_sub = orb_subscribe(ORB_ID(input_rc));
|
||||||
_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
|
_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
|
||||||
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||||
|
|
Loading…
Reference in New Issue