diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-06-30 00:38:01 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-06-30 00:38:01 +0200 |
commit | 7d59ee683961d9b63476cabed56e6aab98a4b392 (patch) | |
tree | 126ff276ae4bd61f504bb5bd475e5263d79b2b74 /src/modules/att_pos_estimator_ekf/KalmanNav.hpp | |
parent | 582ee13d2a1d485f0851986672e0abf2f106867a (diff) | |
download | px4-firmware-7d59ee683961d9b63476cabed56e6aab98a4b392.tar.gz px4-firmware-7d59ee683961d9b63476cabed56e6aab98a4b392.tar.bz2 px4-firmware-7d59ee683961d9b63476cabed56e6aab98a4b392.zip |
Fixed yaw estimate, minor comment and code style improvements. Added option for upfront init, prepared process for removal (later) of sensors app and subscription to individual topics. Prototyping rework of params / subscriptions to resolve GCC 4.7 incompatibility of control lib (or rather the fact that we need to work around something which looks like a compiler bug)
Diffstat (limited to 'src/modules/att_pos_estimator_ekf/KalmanNav.hpp')
-rw-r--r-- | src/modules/att_pos_estimator_ekf/KalmanNav.hpp | 17 |
1 files changed, 15 insertions, 2 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index c2bf18115..49d0d157d 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -56,6 +56,10 @@ #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/parameter_update.h> +#include <drivers/drv_accel.h> +#include <drivers/drv_gyro.h> +#include <drivers/drv_mag.h> + #include <drivers/drv_hrt.h> #include <poll.h> #include <unistd.h> @@ -78,6 +82,9 @@ public: */ virtual ~KalmanNav() {}; + + math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz); + /** * The main callback function for the class */ @@ -136,6 +143,11 @@ protected: // publications control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */ control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */ + + int _accel_sub; /**< Accelerometer subscription */ + int _gyro_sub; /**< Gyroscope subscription */ + int _mag_sub; /**< Magnetometer subscription */ + // time stamps uint64_t _pubTimeStamp; /**< output data publication time stamp */ uint64_t _predictTimeStamp; /**< prediction time stamp */ @@ -151,7 +163,8 @@ protected: enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */ float phi, theta, psi; /**< 3-2-1 euler angles */ float vN, vE, vD; /**< navigation velocity, m/s */ - double lat, lon, alt; /**< lat, lon, alt, radians */ + double lat, lon; /**< lat, lon radians */ + float alt; /**< altitude, meters */ // parameters control::BlockParam<float> _vGyro; /**< gyro process noise */ control::BlockParam<float> _vAccel; /**< accelerometer process noise */ |