diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-14 11:40:08 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-14 11:40:08 +0200 |
commit | 92a1fab0fd9e95737258c30fc423e839652edb72 (patch) | |
tree | a19288747ea7a7aaf060caca1b38947d39ce9acf /apps/mavlink | |
parent | ebbdbac97b500075407bc051e38a8c602c202ac0 (diff) | |
download | px4-firmware-92a1fab0fd9e95737258c30fc423e839652edb72.tar.gz px4-firmware-92a1fab0fd9e95737258c30fc423e839652edb72.tar.bz2 px4-firmware-92a1fab0fd9e95737258c30fc423e839652edb72.zip |
Integrated optical flow
Diffstat (limited to 'apps/mavlink')
-rw-r--r-- | apps/mavlink/mavlink.c | 52 |
1 files changed, 46 insertions, 6 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 1e6156557..66b3865cc 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -72,6 +72,7 @@ #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/optical_flow.h> #include "waypoints.h" #include "mavlink_log.h" @@ -126,12 +127,19 @@ static int ardrone_motors_pub = -1; static int cmd_pub = -1; static int global_pos_sub = -1; static int local_pos_sub = -1; +static int flow_pub = -1; static int global_position_setpoint_pub = -1; static int local_position_setpoint_pub = -1; static bool mavlink_hil_enabled = false; static char mavlink_message_string[51] = {0}; +/* interface mode */ +static enum { + MAVLINK_INTERFACE_MODE_OFFBOARD, + MAVLINK_INTERFACE_MODE_ONBOARD +} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; + /* 3: Define waypoint helper functions */ void mavlink_wpm_send_message(mavlink_message_t *msg); @@ -828,6 +836,7 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) void handleMessage(mavlink_message_t *msg) { if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + mavlink_command_long_t cmd_mavlink; mavlink_msg_command_long_decode(msg, &cmd_mavlink); @@ -863,12 +872,39 @@ void handleMessage(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = cmd_mavlink.confirmation; - /* Publish */ + /* check if topic is advertised */ + if (cmd_pub <= 0) { + cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } + /* publish */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); } } } + if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { + mavlink_optical_flow_t flow; + mavlink_msg_optical_flow_decode(msg, &flow); + + struct optical_flow_s f; + + f.timestamp = flow.time_usec; + f.flow_raw_x = flow.flow_x; + f.flow_raw_y = flow.flow_y; + f.flow_comp_x_m = flow.flow_comp_m_x; + f.flow_comp_y_m = flow.flow_comp_m_y; + f.ground_distance_m = flow.ground_distance; + f.quality = flow.quality; + f.sensor_id = flow.sensor_id; + + /* check if topic is advertised */ + if (flow_pub <= 0) { + flow_pub = orb_advertise(ORB_ID(optical_flow), &flow); + } + /* publish */ + orb_publish(ORB_ID(optical_flow), flow_pub, &flow); + } + if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { /* Set mode on request */ mavlink_set_mode_t new_mode; @@ -909,6 +945,10 @@ void handleMessage(mavlink_message_t *msg) ardrone_motors.counter++; ardrone_motors.timestamp = hrt_absolute_time(); + /* check if topic has to be advertised */ + if (ardrone_motors_pub <= 0) { + ardrone_motors_pub = orb_advertise(ORB_ID(ardrone_motors_setpoint), &ardrone_motors); + } /* Publish */ orb_publish(ORB_ID(ardrone_motors_setpoint), ardrone_motors_pub, &ardrone_motors); } @@ -1155,7 +1195,7 @@ int mavlink_main(int argc, char *argv[]) /* Send raw sensor values at 10 Hz / every 100 ms */ mavlink_message_intervals[MAVLINK_MSG_ID_RAW_IMU] = 100; - //default values for arguments + /* default values for arguments */ char *uart_name = "/dev/ttyS0"; int baudrate = 57600; const char *commandline_usage = "\tusage: %s -d <devicename> -b <baudrate> [-e/--exit-allowed]\n\t\tdefault: -d %s -b %i\n"; @@ -1195,6 +1235,10 @@ int mavlink_main(int argc, char *argv[]) if (strcmp(argv[i], "-e") == 0 || strcmp(argv[i], "--exit-allowed") == 0) { mavlink_link_termination_allowed = true; } + + if (strcmp(argv[i], "-o") == 0 || strcmp(argv[i], "--onboard") == 0) { + mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD; + } } struct termios uart_config_original; @@ -1211,10 +1255,6 @@ int mavlink_main(int argc, char *argv[]) /* Flush UART */ fflush(stdout); - /* topics to advertise */ - ardrone_motors_pub = orb_advertise(ORB_ID(ardrone_motors_setpoint), &ardrone_motors); - cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - /* topics to subscribe globally */ /* subscribe to ORB for global position */ global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); |