From 7c7b5e475d7971bda40cd2bd6ecd41d7a512b26e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Nov 2013 16:14:15 +0100 Subject: Waypoints: Store waypoints in mission topic (WIP, in parallel for now) --- src/modules/mavlink/waypoints.c | 78 ++++++++++++++++++++++++++++++++++++----- 1 file changed, 69 insertions(+), 9 deletions(-) diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 7e4a2688f..5fdbd57e1 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. * Author: @author Petri Tanskanen * @author Lorenz Meier * @author Thomas Gubler @@ -49,6 +49,9 @@ #include "missionlib.h" #include "waypoints.h" #include "util.h" +#include +#include + #ifndef FM_PI #define FM_PI 3.1415926535897932384626433832795f @@ -58,10 +61,39 @@ bool debug = false; bool verbose = false; -#define MAVLINK_WPM_NO_PRINTF +//#define MAVLINK_WPM_NO_PRINTF +#define MAVLINK_WPM_VERBOSE 1 uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; +void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +{ + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->yaw = mavlink_mission_item->param4*M_DEG_TO_RAD_F; + mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); + mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->nav_cmd = NAV_CMD_WAYPOINT; // TODO correct + mission_item->radius = mavlink_mission_item->param1; + mission_item->time_inside = mavlink_mission_item->param2; +} + +void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, const uint16_t seq, mavlink_mission_item_t *mavlink_mission_item) +{ + + mavlink_mission_item->x = (float)mission_item->lat; + mavlink_mission_item->y = (float)mission_item->lon; + mavlink_mission_item->z = mission_item->altitude; + mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; + mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; + mavlink_mission_item->command = MAV_CMD_NAV_WAYPOINT; // TODO add + mavlink_mission_item->param1 = mission_item->radius; + mavlink_mission_item->param2 = mission_item->time_inside; + + mavlink_mission_item->seq = seq; +} + void mavlink_wpm_init(mavlink_wpm_storage *state) { // Set all waypoints to zero @@ -185,11 +217,12 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } -void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, mavlink_mission_item_t *wp) { - if (seq < wpm->size) { + + // if (seq < wpm->size) { mavlink_message_t msg; - mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); + // mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); wp->target_system = wpm->current_partner_sysid; wp->target_component = wpm->current_partner_compid; mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp); @@ -199,9 +232,9 @@ void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); - } + // } else { + // if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); + // } } void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) @@ -523,6 +556,9 @@ 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) { + static orb_advert_t mission_pub = -1; + static struct mission_s mission; + uint64_t now = mavlink_missionlib_get_system_timestamp(); switch (msg->msgid) { @@ -671,7 +707,10 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; wpm->current_wp_id = wpr.seq; - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpr.seq); + + mavlink_mission_item_t wp; + map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], wpr.seq, &wp); + mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp); } else { // if (verbose) @@ -794,6 +833,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->current_partner_compid = msg->compid; wpm->current_count = wpc.count; + /* prepare mission topic */ + mission.count = wpc.count; + mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * mission.count); + + if (!mission.items) { + mission.count = 0; + /* XXX reject waypoints if this fails */ + warnx("no free RAM to allocate mission, rejecting any waypoints"); + } + #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY"); #else @@ -933,7 +982,18 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi for (i = 0; i < wpm->current_count; ++i) { wpm->waypoints[i] = wpm->rcv_waypoints[i]; + map_mavlink_mission_item_to_mission_item(&wpm->rcv_waypoints[i], &mission.items[i]); } + // TODO: update count? + + /* Initialize mission publication if necessary */ + if (mission_pub < 0) { + mission_pub = orb_advertise(ORB_ID(mission), &mission); + + } else { + orb_publish(ORB_ID(mission), mission_pub, &mission); + } + wpm->size = wpm->current_count; -- cgit v1.2.3