diff options
Diffstat (limited to 'apps/mavlink/orb_listener.c')
| -rw-r--r-- | apps/mavlink/orb_listener.c | 23 |
1 files changed, 23 insertions, 0 deletions
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index a067460d8..e6ec77bf6 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -114,6 +114,7 @@ static void l_manual_control_setpoint(struct listener *l); static void l_vehicle_attitude_controls(struct listener *l); static void l_debug_key_value(struct listener *l); static void l_optical_flow(struct listener *l); +static void l_vehicle_rates_setpoint(struct listener *l); struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -136,6 +137,7 @@ struct listener listeners[] = { {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0}, + {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -409,6 +411,23 @@ l_attitude_setpoint(struct listener *l) } void +l_vehicle_rates_setpoint(struct listener *l) +{ + struct vehicle_rates_setpoint_s rates_sp; + + /* copy local position data into local buffer */ + orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp); + + if (gcs_link) + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0, + rates_sp.timestamp/1000, + rates_sp.roll, + rates_sp.pitch, + rates_sp.yaw, + rates_sp.thrust); +} + +void l_actuator_outputs(struct listener *l) { struct actuator_outputs_s act_outputs; @@ -695,6 +714,10 @@ uorb_receive_start(void) mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */ + /* --- RATES SETPOINT VALUE --- */ + mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */ + /* --- ACTUATOR OUTPUTS --- */ mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); |