AP_InertialSensor: added PX4 gyro/accel driver
This commit is contained in:
Andrew Tridgell
parent
2e2f96d721
commit
ae09b31176
@ -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__
|
||||
|
||||
168
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
Normal file
168
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
Normal file
@ -0,0 +1,168 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
#include "AP_InertialSensor_PX4.h"
|
||||
|
||||
const extern AP_HAL::HAL& hal;
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
|
||||
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
|
||||
|
||||
47
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
Normal file
47
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
Normal file
@ -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 <AP_HAL.h>
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
|
||||
#include <AP_Progmem.h>
|
||||
#include "AP_InertialSensor.h"
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
|
||||
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__
|
||||
Loading…
Reference in New Issue
Block a user