diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-22 00:40:45 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-22 00:40:45 +0200 |
| commit | 752a0a562564ccc6f7d49ceebe810de7e6a6d358 (patch) | |
| tree | 4cbf58d1fbda81009b289b641882eff6895a5045 /src/modules/navigator/navigator_main.cpp | |
| parent | 1a14ff250e5a2ead69576762fd5b7f176c4b6fac (diff) | |
| download | px4-firmware-752a0a562564ccc6f7d49ceebe810de7e6a6d358.tar.gz px4-firmware-752a0a562564ccc6f7d49ceebe810de7e6a6d358.tar.bz2 px4-firmware-752a0a562564ccc6f7d49ceebe810de7e6a6d358.zip | |
add obc gps failure mode
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 16 |
1 files changed, 16 insertions, 0 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bc9951bf2..e0913bb57 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -110,6 +110,7 @@ Navigator::Navigator() : _param_update_sub(-1), _pos_sp_triplet_pub(-1), _mission_result_pub(-1), + _att_sp_pub(-1), _vstatus{}, _control_mode{}, _global_pos{}, @@ -119,6 +120,7 @@ Navigator::Navigator() : _nav_caps{}, _pos_sp_triplet{}, _mission_result{}, + _att_sp{}, _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, @@ -609,3 +611,17 @@ Navigator::publish_mission_result() _mission_result.reached = false; _mission_result.finished = false; } + +void +Navigator::publish_att_sp() +{ + /* lazily publish the attitude sp only once available */ + if (_att_sp_pub > 0) { + /* publish att sp*/ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + /* advertise and publish */ + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } +} |