dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1821 from PX4/mcposcontrolmultiplatform1703 

port #1703 to multiplatform
This commit is contained in:
Thomas Gubler 2015-02-24 20:30:18 +01:00
parent 6e07c8c9e4 4f38512334
commit eb5278d12c
2 changed files with 12 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
View File

@ -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;

1
src/modules/mc_pos_control_multiplatform/mc_pos_control.h
View File

@ -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(); }