diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-07 09:47:49 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-07 09:47:49 +0100 |
commit | 395033eeb0b76eacef78d489a69af8442fadf947 (patch) | |
tree | 8ff59162310d46431dcdf8f69222912d7b87f5b1 | |
parent | 8bd532c8556d7f99d1d66c161f182aeebd1ab325 (diff) | |
download | px4-firmware-395033eeb0b76eacef78d489a69af8442fadf947.tar.gz px4-firmware-395033eeb0b76eacef78d489a69af8442fadf947.tar.bz2 px4-firmware-395033eeb0b76eacef78d489a69af8442fadf947.zip |
Switch to 21 state version, publish local position as well
-rw-r--r-- | src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 41 |
1 files changed, 40 insertions, 1 deletions
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 44c25c407..e6747604b 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -35,6 +35,8 @@ * @file fw_att_pos_estimator_main.cpp * Implementation of the attitude and position estimator. * + * @author Paul Riseborough <p_riseborough@live.com.au> + * @author Lorenz Meier <lm@inf.ethz.ch> */ #include <nuttx/config.h> @@ -55,6 +57,7 @@ #include <uORB/uORB.h> #include <uORB/topics/airspeed.h> #include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude_setpoint.h> @@ -129,6 +132,7 @@ private: orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ + orb_advert_t _local_pos_pub; /**< position in local frame */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -139,6 +143,7 @@ private: struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct vehicle_local_position_s _local_pos; /**< local vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ struct gyro_scale _gyro_offsets; @@ -227,6 +232,7 @@ FixedwingEstimator::FixedwingEstimator() : /* publications */ _att_pub(-1), _global_pos_pub(-1), + _local_pos_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw_att_pos_estimator")), @@ -603,6 +609,15 @@ FixedwingEstimator::task_main() if (_initialized) { + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + math::Quaternion q(states[0], states[1], states[2], states[3]); math::Dcm R(q); math::EulerAngles euler(R); @@ -641,9 +656,33 @@ FixedwingEstimator::task_main() _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } + _local_pos.timestamp = _gyro.timestamp; + _local_pos.x = states[7]; + _local_pos.y = states[8]; + _local_pos.z = states[9]; + + _local_pos.vx = states[4]; + _local_pos.vy = states[5]; + _local_pos.vz = states[6]; + + _local_pos.xy_valid = true; + _local_pos.z_valid = true; + _local_pos.v_xy_valid = true; + _local_pos.v_z_valid = true; + + /* lazily publish the local position only once available */ + if (_local_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); + + } else { + /* advertise and publish */ + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); + } + _global_pos.timestamp = _gyro.timestamp; - // /* lazily publish the position only once available */ + // /* lazily publish the global position only once available */ // if (_global_pos_pub > 0) { // /* publish the attitude setpoint */ // orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); |