diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-05 11:37:17 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-05 11:37:17 +0200 |
commit | 84e11a0cac53f753f65b0bea4659e1f2d9c0b35e (patch) | |
tree | 05ca5f3573c7cfbc1c43e71a3cbdc5ca58d3cee5 /apps/mavlink/mavlink.c | |
parent | 86a2a4fb9fd2b5ed38b330923823e06b96af01f3 (diff) | |
download | px4-firmware-84e11a0cac53f753f65b0bea4659e1f2d9c0b35e.tar.gz px4-firmware-84e11a0cac53f753f65b0bea4659e1f2d9c0b35e.tar.bz2 px4-firmware-84e11a0cac53f753f65b0bea4659e1f2d9c0b35e.zip |
Fixed correct RC loss detection, AR.Drone is now shutting down motors after 1 s of RC loss. Added debug topic.
Diffstat (limited to 'apps/mavlink/mavlink.c')
-rw-r--r-- | apps/mavlink/mavlink.c | 45 |
1 files changed, 38 insertions, 7 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index ef0eab130..0d9be58e9 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -75,6 +75,7 @@ #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/debug_key_value.h> #include <systemlib/param/param.h> #include "waypoints.h" @@ -166,6 +167,7 @@ static struct mavlink_subscriptions { int spa_sub; int spl_sub; int spg_sub; + int debug_key_value; bool initialized; } mavlink_subs = { .sensor_sub = 0, @@ -183,6 +185,7 @@ static struct mavlink_subscriptions { .spa_sub = 0, .spl_sub = 0, .spg_sub = 0, + .debug_key_value = 0, .initialized = false }; @@ -543,6 +546,10 @@ static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int ma case MAVLINK_MSG_ID_MANUAL_CONTROL: /* manual_control_setpoint triggers this message */ if (subs->man_control_sp_sub) orb_set_interval(subs->man_control_sp_sub, min_interval); + break; + case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: + if (subs->debug_key_value) orb_set_interval(subs->debug_key_value, min_interval); + break; default: /* not found */ ret = ERROR; @@ -588,6 +595,7 @@ static void *uorb_receiveloop(void *arg) struct actuator_outputs_s act_outputs; struct manual_control_setpoint_s man_control; struct actuator_controls_s actuators; + struct debug_key_value_s debug; } buf; /* --- SENSORS RAW VALUE --- */ @@ -709,6 +717,13 @@ static void *uorb_receiveloop(void *arg) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- DEBUG VALUE OUTPUT --- */ + /* subscribe to ORB for debug value outputs */ + subs->debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); + fds[fdsc_count].fd = subs->debug_key_value; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* all subscriptions initialized, return success */ subs->initialized = true; @@ -1060,10 +1075,17 @@ static void *uorb_receiveloop(void *arg) /* --- 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]); + /* send, add spaces so that string buffer is at least 10 chars long */ + 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]); + } + + /* --- DEBUG KEY/VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(debug_key_value), subs->debug_key_value, &buf.debug); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, buf.debug.key, buf.debug.value); } } } @@ -1501,6 +1523,7 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 2); /* 200 Hz / 5 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 5); /* 5 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200); } else if (baudrate >= 460800) { @@ -1508,15 +1531,19 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5); + /* 100 Hz / 10 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); /* 50 Hz / 20 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 20); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); } else if (baudrate >= 115200) { /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 20); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); + /* 20 Hz / 50 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); /* 10 Hz / 100 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100); /* 1 Hz */ @@ -1527,6 +1554,8 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 100); /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); + /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); /* 0.2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 5000); @@ -1535,6 +1564,8 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 1000); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); + /* 1 Hz / 1000 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); /* 0.5 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); /* 0.1 Hz */ |