Browse Source

Changed to actuators effective in mavlink app

sbg
Lorenz Meier 12 years ago
parent
commit
7777d4416d
  1. 1
      apps/mavlink/mavlink.c
  2. 14
      apps/mavlink/orb_listener.c
  3. 2
      apps/mavlink/orb_topics.h

1
apps/mavlink/mavlink.c

@ -745,6 +745,7 @@ int mavlink_main(int argc, char *argv[]) @@ -745,6 +745,7 @@ int mavlink_main(int argc, char *argv[])
thread_should_exit = true;
while (thread_running) {
usleep(200000);
printf(".");
}
warnx("terminated.");
exit(0);

14
apps/mavlink/orb_listener.c

@ -566,27 +566,27 @@ l_manual_control_setpoint(struct listener *l) @@ -566,27 +566,27 @@ l_manual_control_setpoint(struct listener *l)
void
l_vehicle_attitude_controls(struct listener *l)
{
struct actuator_controls_s actuators;
struct actuator_controls_effective_s actuators;
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators);
if (gcs_link) {
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl0 ",
"eff ctrl0 ",
actuators.control[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl1 ",
"eff ctrl1 ",
actuators.control[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl2 ",
"eff ctrl2 ",
actuators.control[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"ctrl3 ",
"eff ctrl3 ",
actuators.control[3]);
}
}
@ -739,7 +739,7 @@ uorb_receive_start(void) @@ -739,7 +739,7 @@ uorb_receive_start(void)
orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */
/* --- ACTUATOR CONTROL VALUE --- */
mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */
/* --- DEBUG VALUE OUTPUT --- */

2
apps/mavlink/orb_topics.h

@ -55,7 +55,7 @@ @@ -55,7 +55,7 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_rc_input.h>

Loading…
Cancel
Save