diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-22 12:21:29 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-24 20:29:18 +0100 |
commit | 4f385123340ce88fed4d441f14986d4769fa97cd (patch) | |
tree | a7d33909a2da1db1b86c0885daa157436ff5b573 /src/modules/mc_pos_control_multiplatform | |
parent | 6e07c8c9e4564f3f9677ddee0a023d0003913212 (diff) | |
download | px4-firmware-4f385123340ce88fed4d441f14986d4769fa97cd.tar.gz px4-firmware-4f385123340ce88fed4d441f14986d4769fa97cd.tar.bz2 px4-firmware-4f385123340ce88fed4d441f14986d4769fa97cd.zip |
port #1703 to multiplatform
Diffstat (limited to 'src/modules/mc_pos_control_multiplatform')
-rw-r--r-- | src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp | 12 | ||||
-rw-r--r-- | src/modules/mc_pos_control_multiplatform/mc_pos_control.h | 1 |
2 files changed, 12 insertions, 1 deletions
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index 391558dcc..e6a7ee8a6 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -106,7 +106,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0); _armed = _n.subscribe<px4_actuator_armed>(0); _local_pos = _n.subscribe<px4_vehicle_local_position>(0); - _pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(0); + _pos_sp_triplet = _n.subscribe<px4_position_setpoint_triplet>(&MulticopterPositionControl::handle_position_setpoint_triplet, this, 0); _local_pos_sp = _n.subscribe<px4_vehicle_local_position_setpoint>(0); _global_vel_sp = _n.subscribe<px4_vehicle_global_velocity_setpoint>(0); @@ -554,6 +554,16 @@ void MulticopterPositionControl::handle_parameter_update(const px4_parameter_upd parameters_update(); } +void MulticopterPositionControl::handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg) +{ + /* Make sure that the position setpoint is valid */ + if (!isfinite(_pos_sp_triplet->data().current.lat) || + !isfinite(_pos_sp_triplet->data().current.lon) || + !isfinite(_pos_sp_triplet->data().current.alt)) { + _pos_sp_triplet->data().current.valid = false; + } +} + void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) { static bool reset_int_z = true; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index 245b5f5a9..b754ccf01 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -75,6 +75,7 @@ public: /* Callbacks for topics */ void handle_vehicle_attitude(const px4_vehicle_attitude &msg); void handle_parameter_update(const px4_parameter_update &msg); + void handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg); void spin() { _n.spin(); } |