From ae09b31176f5596c7c09e2b4131c7e9581734446 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Jan 2013 14:25:57 +1100 Subject: [PATCH] AP_InertialSensor: added PX4 gyro/accel driver --- .../AP_InertialSensor/AP_InertialSensor.h | 1 + .../AP_InertialSensor_PX4.cpp | 168 ++++++++++++++++++ .../AP_InertialSensor/AP_InertialSensor_PX4.h | 47 +++++ 3 files changed, 216 insertions(+) create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_PX4.h diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 46145732d7..3697dbcc2b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -172,6 +172,7 @@ protected: #include "AP_InertialSensor_Oilpan.h" #include "AP_InertialSensor_MPU6000.h" #include "AP_InertialSensor_Stub.h" +#include "AP_InertialSensor_PX4.h" #include "AP_InertialSensor_UserInteract_Stream.h" #endif // __AP_INERTIAL_SENSOR_H__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp new file mode 100644 index 0000000000..4e4b0a644f --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp @@ -0,0 +1,168 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#include "AP_InertialSensor_PX4.h" + +const extern AP_HAL::HAL& hal; + +#include +#include +#include +#include + +#include +#include +#include +#include + +uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) { + uint16_t rate_hz; + int fd; + + switch (sample_rate) { + case RATE_50HZ: + rate_hz = 50; + break; + case RATE_100HZ: + rate_hz = 100; + break; + case RATE_200HZ: + default: + rate_hz = 200; + break; + } + + // init accelerometers + fd = open(ACCEL_DEVICE_PATH, 0); + if (fd < 0) { + hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH); + } + + /* set the accel internal sampling rate */ + ioctl(fd, ACCELIOCSSAMPLERATE, rate_hz); + + /* set the driver poll rate */ + ioctl(fd, SENSORIOCSPOLLRATE, rate_hz); + + close(fd); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + + // init gyros + fd = open(GYRO_DEVICE_PATH, 0); + if (fd < 0) { + hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH); + } + + /* set the gyro internal sampling rate */ + ioctl(fd, GYROIOCSSAMPLERATE, rate_hz); + + /* set the driver poll rate */ + ioctl(fd, SENSORIOCSPOLLRATE, rate_hz); + + close(fd); + _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + + return AP_PRODUCT_ID_PX4; +} + +/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ + +bool AP_InertialSensor_PX4::update(void) +{ + while (num_samples_available() == 0) { + hal.scheduler->delay_microseconds(1); + } + uint32_t now = hal.scheduler->micros(); + _delta_time_usec = now - _last_update_usec; + _last_update_usec = now; + + Vector3f accel_scale = _accel_scale.get(); + + _accel.x = accel_scale.x * _raw_sensors.accelerometer_m_s2[0] / _raw_sensors.accelerometer_counter; + _accel.y = - accel_scale.y * _raw_sensors.accelerometer_m_s2[1] / _raw_sensors.accelerometer_counter; + _accel.z = - accel_scale.z * _raw_sensors.accelerometer_m_s2[2] / _raw_sensors.accelerometer_counter; + _accel -= _accel_offset; + + _gyro.x = _raw_sensors.gyro_rad_s[0] / _raw_sensors.gyro_counter; + _gyro.y = - _raw_sensors.gyro_rad_s[1] / _raw_sensors.gyro_counter; + _gyro.z = - _raw_sensors.gyro_rad_s[2] / _raw_sensors.gyro_counter; + _gyro -= _gyro_offset; + + memset(&_raw_sensors, 0, sizeof(_raw_sensors)); + + return true; +} + +bool AP_InertialSensor_PX4::new_data_available(void) +{ + return num_samples_available() > 0; +} + + +float AP_InertialSensor_PX4::temperature(void) +{ + return 0.0; +} + +uint32_t AP_InertialSensor_PX4::get_delta_time_micros(void) +{ + return _delta_time_usec; +} + +uint32_t AP_InertialSensor_PX4::get_last_sample_time_micros(void) +{ + return _last_update_usec; +} + +float AP_InertialSensor_PX4::get_gyro_drift_rate(void) +{ + // 0.5 degrees/second/minute + return ToRad(0.5/60); +} + +uint16_t AP_InertialSensor_PX4::num_samples_available(void) +{ + bool accel_updated=false; + bool gyro_updated =false; + + orb_check(_accel_sub, &accel_updated); + if (accel_updated) { + struct accel_report accel_report; + + orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); + + _raw_sensors.accelerometer_m_s2[0] += accel_report.x; + _raw_sensors.accelerometer_m_s2[1] += accel_report.y; + _raw_sensors.accelerometer_m_s2[2] += accel_report.z; + + _raw_sensors.accelerometer_raw[0] = accel_report.x_raw; + _raw_sensors.accelerometer_raw[1] = accel_report.y_raw; + _raw_sensors.accelerometer_raw[2] = accel_report.z_raw; + + _raw_sensors.accelerometer_counter++; + } + + orb_check(_gyro_sub, &gyro_updated); + + if (gyro_updated) { + struct gyro_report gyro_report; + + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); + + _raw_sensors.gyro_rad_s[0] += gyro_report.x; + _raw_sensors.gyro_rad_s[1] += gyro_report.y; + _raw_sensors.gyro_rad_s[2] += gyro_report.z; + + _raw_sensors.gyro_raw[0] = gyro_report.x_raw; + _raw_sensors.gyro_raw[1] = gyro_report.y_raw; + _raw_sensors.gyro_raw[2] = gyro_report.z_raw; + + _raw_sensors.gyro_counter++; + } + + return min(_raw_sensors.accelerometer_counter, _raw_sensors.gyro_counter); +} + +#endif // CONFIG_HAL_BOARD + diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h new file mode 100644 index 0000000000..93ed71ccdd --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_PX4.h @@ -0,0 +1,47 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef __AP_INERTIAL_SENSOR_PX4_H__ +#define __AP_INERTIAL_SENSOR_PX4_H__ + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + +#include +#include "AP_InertialSensor.h" +#include +#include +#include +#include + +class AP_InertialSensor_PX4 : public AP_InertialSensor +{ +public: + + AP_InertialSensor_PX4() {} + + /* Concrete implementation of AP_InertialSensor functions: */ + bool update(); + bool new_data_available(); + float temperature(); + uint32_t get_delta_time_micros(); + uint32_t get_last_sample_time_micros(); + float get_gyro_drift_rate(); + uint16_t num_samples_available(); + +private: + uint16_t _init_sensor( Sample_rate sample_rate ); + uint32_t _last_update_usec; + uint32_t _delta_time_usec; + + // accelerometer ORB subscription handle + int _accel_sub; + + // gyro ORB subscription handle + int _gyro_sub; + + // raw sensor values, combined structure + struct sensor_combined_s _raw_sensors; + +}; +#endif +#endif // __AP_INERTIAL_SENSOR_PX4_H__