From 962a3464a624deef390fd42e179537cc0590b903 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Aug 2012 17:24:48 +0200 Subject: Minor cleanups in WP handling --- apps/mavlink/mavlink.c | 130 ++++++++++++++++++++++++++++++++++------------- apps/mavlink/waypoints.c | 14 ++--- apps/mavlink/waypoints.h | 4 ++ 3 files changed, 106 insertions(+), 42 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 74f7b2099..6484036ac 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -45,6 +45,7 @@ #include #include #include +#include #include "mavlink_bridge_header.h" #include #include @@ -81,7 +82,7 @@ static bool mavlink_link_termination_allowed = false; static bool mavlink_exit_requested = false; static int system_type = MAV_TYPE_FIXED_WING; -mavlink_system_t mavlink_system = {100, 50}; // System ID, 1-255, Component/Subsystem ID, 1-255 +mavlink_system_t mavlink_system = {100, 50, 0, 0, 0, 0}; // System ID, 1-255, Component/Subsystem ID, 1-255 static uint8_t chan = MAVLINK_COMM_0; static mavlink_status_t status; @@ -92,22 +93,33 @@ static pthread_t uorb_receive_thread; static uint16_t mavlink_message_intervals[256]; /**< intervals at which to send MAVLink packets */ /* Allocate storage space for waypoints */ mavlink_wpm_storage wpm_s; -/* Global position */ -static struct vehicle_global_position_s global_pos = {0}; -/* Local position */ -static struct vehicle_local_position_s local_pos = {0}; -/* Vehicle status */ -static struct vehicle_status_s v_status = {0}; -/* RC channels */ -static struct rc_channels_s rc = {0}; + +/** Global position */ +static struct vehicle_global_position_s global_pos; + +/** Local position */ +static struct vehicle_local_position_s local_pos; + +/** Vehicle status */ +static struct vehicle_status_s v_status; + +/** RC channels */ +static struct rc_channels_s rc; /* HIL publishers */ static int pub_hil_attitude = -1; -static struct vehicle_attitude_s hil_attitude = {0}; -static struct vehicle_global_position_s hil_global_pos = {0}; -static struct fixedwing_control_s fw_control = {0}; -static struct ardrone_motors_setpoint_s ardrone_motors = {0}; -static struct vehicle_command_s vcmd = {0}; + +/** HIL attitude */ +static struct vehicle_attitude_s hil_attitude; + +static struct vehicle_global_position_s hil_global_pos; + +static struct fixedwing_control_s fw_control; + +static struct ardrone_motors_setpoint_s ardrone_motors; + +static struct vehicle_command_s vcmd; + static int pub_hil_global_pos = -1; static int ardrone_motors_pub = -1; static int cmd_pub = -1; @@ -128,6 +140,20 @@ void mavlink_missionlib_send_message(mavlink_message_t *msg); void mavlink_missionlib_send_gcs_string(const char *string); uint64_t mavlink_missionlib_get_system_timestamp(void); +void handleMessage(mavlink_message_t *msg); + +/** + * Enable / disable Hardware in the Loop simulation mode. + */ +int set_hil_on_off(uint8_t vehicle_mode); + +/** + * Translate the custom state into standard mavlink modes and state. + */ +void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t *mavlink_state, uint8_t *mavlink_mode); + +int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + /* 4: Include waypoint protocol */ #include "waypoints.h" mavlink_wpm_storage *wpm; @@ -168,7 +194,7 @@ void mavlink_missionlib_send_gcs_string(const char *string) } } -/* +/** * Get system time since boot in microseconds * * @return the system time since boot in microseconds @@ -178,30 +204,50 @@ uint64_t mavlink_missionlib_get_system_timestamp() return hrt_absolute_time(); } +/** + * This callback is executed each time a waypoint changes. + * + * It publishes the vehicle_global_position_setpoint_s or the + * vehicle_local_position_setpoint_s topic, depending on the type of waypoint + */ extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, float param2, float param3, float param4, float param5_lat_x, float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) { + char buf[50] = {0}; + /* Update controller setpoints */ if (frame == (int)MAV_FRAME_GLOBAL) { /* global, absolute waypoint */ struct vehicle_global_position_setpoint_s sp; - sp.lat = param5_lat_x * 1e7; - sp.lon = param6_lon_y * 1e7; + sp.lat = param5_lat_x * 1e7f; + sp.lon = param6_lon_y * 1e7f; sp.altitude = param7_alt_z; sp.altitude_is_relative = false; - sp.yaw = (param4 / 180.0f) * ((float)M_PI) - ((float)M_PI); - orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); + sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + /* Initialize publication if necessary */ + if (global_position_setpoint_pub < 0) { + global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); + } else { + orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); + } + sprintf(buf, "[mavlink mp] Heading towards WP #%i (lat: %f/lon %f/alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { /* global, relative alt (in relation to HOME) waypoint */ struct vehicle_global_position_setpoint_s sp; - sp.lat = param5_lat_x * 1e7; - sp.lon = param6_lon_y * 1e7; + sp.lat = param5_lat_x * 1e7f; + sp.lon = param6_lon_y * 1e7f; sp.altitude = param7_alt_z; sp.altitude_is_relative = true; - sp.yaw = (param4 / 180.0f) * ((float)M_PI) - ((float)M_PI); - orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); + sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + /* Initialize publication if necessary */ + if (global_position_setpoint_pub < 0) { + global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); + } else { + orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); + } + sprintf(buf, "[mavlink mp] Heading towards WP #%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { /* local, absolute waypoint */ @@ -209,19 +255,23 @@ extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float pa sp.x = param5_lat_x; sp.y = param6_lon_y; sp.z = param7_alt_z; - sp.yaw = (param4 / 180.0f) * ((float)M_PI) - ((float)M_PI); - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); + sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; + /* Initialize publication if necessary */ + if (local_position_setpoint_pub < 0) { + local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); + } else { + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); + } + sprintf(buf, "[mavlink mp] Heading towards WP #%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } - - //printf("[mavlink mp] new setpoint: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); + + mavlink_missionlib_send_gcs_string(buf); + printf("%s\n", buf); + //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); } -void handleMessage(mavlink_message_t *msg); -/** - * Enable / disable Hardware in the Loop simulation mode. - */ int set_hil_on_off(uint8_t vehicle_mode) { int ret = OK; @@ -261,9 +311,6 @@ int set_hil_on_off(uint8_t vehicle_mode) return ret; } -/** - * Translate the custom state into standard mavlink modes and state. - */ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t *mavlink_state, uint8_t *mavlink_mode) { //TODO: Make this correct @@ -1059,10 +1106,21 @@ int mavlink_main(int argc, char *argv[]) { wpm = &wpm_s; - // print text + /* initialize global data structs */ + memset(&global_pos, 0, sizeof(global_pos)); + memset(&local_pos, 0, sizeof(local_pos)); + memset(&v_status, 0, sizeof(v_status)); + memset(&rc, 0, sizeof(rc)); + memset(&hil_attitude, 0, sizeof(hil_attitude)); + memset(&hil_global_pos, 0, sizeof(hil_global_pos)); + memset(&fw_control, 0, sizeof(fw_control)); + memset(&ardrone_motors, 0, sizeof(ardrone_motors)); + memset(&vcmd, 0, sizeof(vcmd)); + + /* print welcome text */ printf("[mavlink] MAVLink v1.0 serial interface starting..\n"); - // create the device node that's used for sending text log messages, etc. + /* reate the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); /* Send attitude at 10 Hz / every 100 ms */ diff --git a/apps/mavlink/waypoints.c b/apps/mavlink/waypoints.c index 977f7d47e..8ed80c817 100644 --- a/apps/mavlink/waypoints.c +++ b/apps/mavlink/waypoints.c @@ -353,9 +353,10 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ { static uint16_t counter; - if (wpm->size > 0 && counter % 10 == 0) { - printf("Currect active waypoint id: %i\n", wpm->current_active_wp_id); - } + // Do not flood the precious wireless link with debug data + // if (wpm->size > 0 && counter % 10 == 0) { + // printf("Currect active waypoint id: %i\n", wpm->current_active_wp_id); + // } if (wpm->current_active_wp_id < wpm->size) { @@ -465,9 +466,10 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio } } - if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) { - mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - } + // Do NOT continously send the current WP, since we're not loosing messages + // if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) { + // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); + // } check_waypoints_reached(now, global_position , local_position); diff --git a/apps/mavlink/waypoints.h b/apps/mavlink/waypoints.h index 6011b16b2..c525f4553 100644 --- a/apps/mavlink/waypoints.h +++ b/apps/mavlink/waypoints.h @@ -125,4 +125,8 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos); +extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, + float param2, float param3, float param4, float param5_lat_x, + float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); + #endif /* WAYPOINTS_H_ */ -- cgit v1.2.3