fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 17:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Piping through manual control channels

Browse Source
This commit is contained in:
Lorenz Meier
2013-10-15 08:39:57 +02:00
parent 16efc0d2d9
commit 3dcd5dbd0e
3 changed files with 32 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensors.cpp
+22
View File
@@ -74,6 +74,7 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h>
@@ -264,6 +265,7 @@ private:
orb_advert_t _sensor_pub; /**< combined sensor data topic */
orb_advert_t _manual_control_pub; /**< manual control signal topic */
orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */
orb_advert_t _rc_pub; /**< raw r/c control topic */
orb_advert_t _battery_pub; /**< battery status */
orb_advert_t _airspeed_pub; /**< airspeed */
@@ -519,6 +521,7 @@ Sensors::Sensors() :
/* publications */
_sensor_pub(-1),
_manual_control_pub(-1),
_actuator_group_3_pub(-1),
_rc_pub(-1),
_battery_pub(-1),
_airspeed_pub(-1),
@@ -1318,6 +1321,7 @@ Sensors::rc_poll()
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
struct manual_control_setpoint_s manual_control;
struct actuator_controls_s actuator_group_3;
/* initialize to default values */
manual_control.roll = NAN;
@@ -1485,6 +1489,16 @@ Sensors::rc_poll()
manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
}
/* copy from mapped manual control to control group 3 */
actuator_group_3.control[0] = manual_control.roll;
actuator_group_3.control[1] = manual_control.pitch;
actuator_group_3.control[2] = manual_control.yaw;
actuator_group_3.control[3] = manual_control.throttle;
actuator_group_3.control[4] = manual_control.flaps;
actuator_group_3.control[5] = manual_control.aux1;
actuator_group_3.control[6] = manual_control.aux2;
actuator_group_3.control[7] = manual_control.aux3;
/* check if ready for publishing */
if (_rc_pub > 0) {
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
@@ -1501,6 +1515,14 @@ Sensors::rc_poll()
} else {
_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
}
/* check if ready for publishing */
if (_actuator_group_3_pub > 0) {
orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
} else {
_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
}
}
}