diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-02 12:21:54 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-02 12:21:54 +0200 |
commit | 8aa41f7d34c6b6cddc59ba9d5ed1567044e66e46 (patch) | |
tree | b607999f4de81e720e62e314b63d07856c83a403 /apps/mavlink/mavlink.c | |
parent | f84f47979b14f9913f9eccad4541a6d1f6adcd58 (diff) | |
download | px4-firmware-8aa41f7d34c6b6cddc59ba9d5ed1567044e66e46.tar.gz px4-firmware-8aa41f7d34c6b6cddc59ba9d5ed1567044e66e46.tar.bz2 px4-firmware-8aa41f7d34c6b6cddc59ba9d5ed1567044e66e46.zip |
Add actuator controls output
Diffstat (limited to 'apps/mavlink/mavlink.c')
-rw-r--r-- | apps/mavlink/mavlink.c | 24 |
1 files changed, 24 insertions, 0 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 2d3903925..d9a201d4b 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -163,6 +163,7 @@ static struct mavlink_subscriptions { int gps_sub; int man_control_sp_sub; int armed_sub; + int actuators_sub; bool initialized; } mavlink_subs = { .sensor_sub = 0, @@ -175,6 +176,7 @@ static struct mavlink_subscriptions { .gps_sub = 0, .man_control_sp_sub = 0, .armed_sub = 0, + .actuators_sub = 0, .initialized = false }; @@ -557,6 +559,7 @@ static void *uorb_receiveloop(void *arg) struct vehicle_attitude_setpoint_s att_sp; struct actuator_outputs_s act_outputs; struct manual_control_setpoint_s man_control; + struct actuator_controls_s actuators; } buf; /* --- SENSORS RAW VALUE --- */ @@ -687,6 +690,14 @@ static void *uorb_receiveloop(void *arg) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- ACTUATOR CONTROL VALUE --- */ + /* subscribe to ORB for actuator control */ + subs->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + orb_set_interval(subs->actuators_sub, 2000); /* 0.5 Hz updates */ + fds[fdsc_count].fd = subs->actuators_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* all subscriptions initialized, return success */ subs->initialized = true; @@ -1041,6 +1052,19 @@ static void *uorb_receiveloop(void *arg) mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll, buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 1, 1, 1, 1); } + + /* --- ACTUATOR ARMED --- */ + if (fds[ifds++].revents & POLLIN) { + } + + /* --- ACTUATOR CONTROL --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs->actuators_sub, &buf.actuators); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl0", buf.actuators.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl1", buf.actuators.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl2", buf.actuators.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl3", buf.actuators.control[3]); + } } } |