From 395033eeb0b76eacef78d489a69af8442fadf947 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 09:47:49 +0100 Subject: Switch to 21 state version, publish local position as well --- .../fw_att_pos_estimator_main.cpp | 41 +++++++++++++++++++++- 1 file changed, 40 insertions(+), 1 deletion(-) (limited to 'src/modules/fw_att_pos_estimator') 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 + * @author Lorenz Meier */ #include @@ -55,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -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); -- cgit v1.2.3