waypoints.c 42.6 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
/*******************************************************************************
 
 Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch
 
 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 ****************************************************************************/

lm's avatar
lm committed
20
#include "waypoints.h"
LM's avatar
LM committed
21
#include <math.h>
22 23 24 25

bool debug = true;
bool verbose = true;

LM's avatar
LM committed
26 27 28
extern mavlink_system_t mavlink_system;
extern mavlink_wpm_storage wpm;

29 30 31
extern void mavlink_missionlib_send_message(mavlink_message_t* msg);
extern void mavlink_missionlib_send_gcs_string(const char* string);
extern uint64_t mavlink_missionlib_get_system_timestamp();
LM's avatar
LM committed
32 33 34


#define MAVLINK_WPM_NO_PRINTF
35

lm's avatar
lm committed
36 37
uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_WAYPOINTPLANNER;

38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72
void mavlink_wpm_init(mavlink_wpm_storage* state)
{
	// Set all waypoints to zero
	
	// Set count to zero
	state->size = 0;
	state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
	state->current_state = MAVLINK_WPM_STATE_IDLE;
	state->current_partner_sysid = 0;
	state->current_partner_compid = 0;
	state->timestamp_lastaction = 0;
	state->timestamp_last_send_setpoint = 0;
	state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
	state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT;
	state->idle = false;      				///< indicates if the system is following the waypoints or is waiting
	state->current_active_wp_id = -1;		///< id of current waypoint
	state->yaw_reached = false;						///< boolean for yaw attitude reached
	state->pos_reached = false;						///< boolean for position reached
	state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
	state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
	
}

/*
 *  @brief Sends an waypoint ack message
 */
void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
{
    mavlink_message_t msg;
    mavlink_waypoint_ack_t wpa;
	
    wpa.target_system = wpm.current_partner_sysid;
    wpa.target_component = wpm.current_partner_compid;
    wpa.type = type;
	
lm's avatar
lm committed
73
    mavlink_msg_waypoint_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa);
74
    mavlink_missionlib_send_message(&msg);
75 76 77 78 79
	
    // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
	
    if (MAVLINK_WPM_TEXT_FEEDBACK)
	{
LM's avatar
LM committed
80
#ifdef MAVLINK_WPM_NO_PRINTF
81
    	mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
LM's avatar
LM committed
82 83 84
#else
		if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
#endif
85
		mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108
	}
}

/*
 *  @brief Broadcasts the new target waypoint and directs the MAV to fly there
 *
 *  This function broadcasts its new active waypoint sequence number and
 *  sends a message to the controller, advising it to fly to the coordinates
 *  of the waypoint with a given orientation
 *
 *  @param seq The waypoint sequence number the MAV should fly to.
 */
void mavlink_wpm_send_waypoint_current(uint16_t seq)
{
    if(seq < wpm.size)
    {
        mavlink_waypoint_t *cur = &(wpm.waypoints[seq]);
		
        mavlink_message_t msg;
        mavlink_waypoint_current_t wpc;
		
        wpc.seq = cur->seq;
		
lm's avatar
lm committed
109
        mavlink_msg_waypoint_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
110
        mavlink_missionlib_send_message(&msg);
111 112 113
		
        // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
		
114
        if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Broadcasted new current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq);
115 116 117
    }
    else
    {
118
        if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: index out of bounds\n");
119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148
    }
}

/*
 *  @brief Directs the MAV to fly to a position
 *
 *  Sends a message to the controller, advising it to fly to the coordinates
 *  of the waypoint with a given orientation
 *
 *  @param seq The waypoint sequence number the MAV should fly to.
 */
void mavlink_wpm_send_setpoint(uint16_t seq)
{
    if(seq < wpm.size)
    {
        mavlink_waypoint_t *cur = &(wpm.waypoints[seq]);
		
        mavlink_message_t msg;
        mavlink_local_position_setpoint_set_t position_control_set_point;
		
        // Send new NED or ENU setpoint to onbaord autopilot
        if (cur->frame == MAV_FRAME_LOCAL_NED || cur->frame == MAV_FRAME_LOCAL_ENU)
        {
            position_control_set_point.target_system = mavlink_system.sysid;
            position_control_set_point.target_component = MAV_COMP_ID_IMU;
            position_control_set_point.x = cur->x;
            position_control_set_point.y = cur->y;
            position_control_set_point.z = cur->z;
            position_control_set_point.yaw = cur->param4;
			
lm's avatar
lm committed
149
            mavlink_msg_local_position_setpoint_set_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &position_control_set_point);
150
            mavlink_missionlib_send_message(&msg);
151 152 153 154 155
			
            // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
        }
        else
        {
156
            if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//// if (verbose) // printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq);
157 158
        }
		
159
        wpm.timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp();
160 161 162
    }
    else
    {
163
        if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n");
164 165 166 167 168 169 170 171 172 173 174 175
    }
}

void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count)
{
    mavlink_message_t msg;
    mavlink_waypoint_count_t wpc;
	
    wpc.target_system = wpm.current_partner_sysid;
    wpc.target_component = wpm.current_partner_compid;
    wpc.count = count;
	
lm's avatar
lm committed
176
    mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
177
    mavlink_missionlib_send_message(&msg);
178
	
179
    if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system);
180 181 182 183 184 185 186 187 188 189 190 191
	
    // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}

void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
{
    if (seq < wpm.size)
    {
        mavlink_message_t msg;
        mavlink_waypoint_t *wp = &(wpm.waypoints[seq]);
        wp->target_system = wpm.current_partner_sysid;
        wp->target_component = wpm.current_partner_compid;
lm's avatar
lm committed
192
        mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp);
193 194
        mavlink_missionlib_send_message(&msg);
        if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
195 196 197 198 199
		
        // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
    }
    else
    {
200
        if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n");
201 202 203 204 205 206 207 208 209 210 211 212
    }
}

void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
{
    if (seq < wpm.max_size)
    {
        mavlink_message_t msg;
        mavlink_waypoint_request_t wpr;
        wpr.target_system = wpm.current_partner_sysid;
        wpr.target_component = wpm.current_partner_compid;
        wpr.seq = seq;
lm's avatar
lm committed
213
        mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr);
214 215
        mavlink_missionlib_send_message(&msg);
        if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system);
216 217 218 219 220
		
        // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
    }
    else
    {
221
        if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n");
222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238
    }
}

/*
 *  @brief emits a message that a waypoint reached
 *
 *  This function broadcasts a message that a waypoint is reached.
 *
 *  @param seq The waypoint sequence number the MAV has reached.
 */
void mavlink_wpm_send_waypoint_reached(uint16_t seq)
{
    mavlink_message_t msg;
    mavlink_waypoint_reached_t wp_reached;
	
    wp_reached.seq = seq;
	
lm's avatar
lm committed
239
    mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached);
240
    mavlink_missionlib_send_message(&msg);
241
	
242
    if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq);
243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282
	
    // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}

//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z)
//{
//    if (seq < wpm.size)
//    {
//        mavlink_waypoint_t *cur = waypoints->at(seq);
//		
//        const PxVector3 A(cur->x, cur->y, cur->z);
//        const PxVector3 C(x, y, z);
//		
//        // seq not the second last waypoint
//        if ((uint16_t)(seq+1) < wpm.size)
//        {
//            mavlink_waypoint_t *next = waypoints->at(seq+1);
//            const PxVector3 B(next->x, next->y, next->z);
//            const float r = (B-A).dot(C-A) / (B-A).lengthSquared();
//            if (r >= 0 && r <= 1)
//            {
//                const PxVector3 P(A + r*(B-A));
//                return (P-C).length();
//            }
//            else if (r < 0.f)
//            {
//                return (C-A).length();
//            }
//            else
//            {
//                return (C-B).length();
//            }
//        }
//        else
//        {
//            return (C-A).length();
//        }
//    }
//    else
//    {
LM's avatar
LM committed
283
//        // if (verbose) // printf("ERROR: index out of bounds\n");
284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300
//    }
//    return -1.f;
//}

float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z)
{
//    if (seq < wpm.size)
//    {
//        mavlink_waypoint_t *cur = waypoints->at(seq);
//		
//        const PxVector3 A(cur->x, cur->y, cur->z);
//        const PxVector3 C(x, y, z);
//		
//        return (C-A).length();
//    }
//    else
//    {
LM's avatar
LM committed
301
//        // if (verbose) // printf("ERROR: index out of bounds\n");
302 303 304 305
//    }
    return -1.f;
}

LM's avatar
LM committed
306
void mavlink_wpm_loop()
307 308
{
    //check for timed-out operations
309
    uint64_t now = mavlink_missionlib_get_system_timestamp();
310 311
    if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
    {
LM's avatar
LM committed
312
#ifdef MAVLINK_WPM_NO_PRINTF
313
    	mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE");
LM's avatar
LM committed
314 315 316 317
#else
		if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state);
#endif
		wpm.current_state = MAVLINK_WPM_STATE_IDLE;
318 319 320 321 322 323 324 325 326 327 328 329 330 331 332
        wpm.current_count = 0;
        wpm.current_partner_sysid = 0;
        wpm.current_partner_compid = 0;
        wpm.current_wp_id = -1;
		
        if(wpm.size == 0)
        {
            wpm.current_active_wp_id = -1;
        }
    }
	
    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);
    }
LM's avatar
LM committed
333 334 335 336 337
}

void mavlink_wpm_message_handler(const mavlink_message_t* msg)
{
	uint64_t now = mavlink_missionlib_get_system_timestamp();
338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382
    switch(msg->msgid)
    {
		case MAVLINK_MSG_ID_ATTITUDE:
        {
            if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size)
            {
                mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]);
                if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED)
                {
                    mavlink_attitude_t att;
                    mavlink_msg_attitude_decode(msg, &att);
                    float yaw_tolerance = wpm.accept_range_yaw;
                    //compare current yaw
                    if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI)
                    {
                        if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
                            wpm.yaw_reached = true;
                    }
                    else if(att.yaw - yaw_tolerance < 0.0f)
                    {
                        float lowerBound = 360.0f + att.yaw - yaw_tolerance;
                        if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
                            wpm.yaw_reached = true;
                    }
                    else
                    {
                        float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI;
                        if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
                            wpm.yaw_reached = true;
                    }
                }
            }
            break;
        }
			
		case MAVLINK_MSG_ID_LOCAL_POSITION:
        {
            if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size)
            {
                mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]);
				
                if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED)
                {
                    mavlink_local_position_t pos;
                    mavlink_msg_local_position_decode(msg, &pos);
LM's avatar
LM committed
383
                    //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
384 385 386 387 388 389 390
					
                    wpm.pos_reached = false;
					
                    // compare current position (given in message) with current waypoint
                    float orbit = wp->param1;
					
                    float dist;
LM's avatar
LM committed
391 392 393 394 395 396 397
//                    if (wp->param2 == 0)
//                    {
//						// FIXME segment distance
//                        //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z);
//                    }
//                    else
//                    {
398
                        dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z);
LM's avatar
LM committed
399
//                    }
400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415
					
                    if (dist >= 0.f && dist <= orbit && wpm.yaw_reached)
                    {
                        wpm.pos_reached = true;
                    }
                }
            }
            break;
        }
			
//		case MAVLINK_MSG_ID_CMD: // special action from ground station
//        {
//            mavlink_cmd_t action;
//            mavlink_msg_cmd_decode(msg, &action);
//            if(action.target == mavlink_system.sysid)
//            {
LM's avatar
LM committed
416
//                // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
417 418 419
//                switch (action.action)
//                {
//						//				case MAV_ACTION_LAUNCH:
LM's avatar
LM committed
420
//						//					// if (verbose) std::cerr << "Launch received" << std::endl;
421 422 423 424 425 426
//						//					current_active_wp_id = 0;
//						//					if (wpm.size>0)
//						//					{
//						//						setActive(waypoints[current_active_wp_id]);
//						//					}
//						//					else
LM's avatar
LM committed
427
//						//						// if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
428 429 430
//						//					break;
//						
//						//				case MAV_ACTION_CONTINUE:
LM's avatar
LM committed
431
//						//					// if (verbose) std::c
432 433 434 435 436 437
//						//					err << "Continue received" << std::endl;
//						//					idle = false;
//						//					setActive(waypoints[current_active_wp_id]);
//						//					break;
//						
//						//				case MAV_ACTION_HALT:
LM's avatar
LM committed
438
//						//					// if (verbose) std::cerr << "Halt received" << std::endl;
439 440 441 442
//						//					idle = true;
//						//					break;
//						
//						//				default:
LM's avatar
LM committed
443
//						//					// if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
444 445 446 447 448 449 450 451 452 453 454
//						//					break;
//                }
//            }
//            break;
//        }
			
		case MAVLINK_MSG_ID_WAYPOINT_ACK:
        {
            mavlink_waypoint_ack_t wpa;
            mavlink_msg_waypoint_ack_decode(msg, &wpa);
			
lm's avatar
lm committed
455
            if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/))
456 457 458 459 460 461 462
            {
                wpm.timestamp_lastaction = now;
				
                if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)
                {
                    if (wpm.current_wp_id == wpm.size-1)
                    {
LM's avatar
LM committed
463
#ifdef MAVLINK_WPM_NO_PRINTF
464
    	mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
LM's avatar
LM committed
465 466 467 468
#else
		if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
#endif
						wpm.current_state = MAVLINK_WPM_STATE_IDLE;
469 470 471 472 473 474
                        wpm.current_wp_id = 0;
                    }
                }
            }
			else
			{
LM's avatar
LM committed
475
#ifdef MAVLINK_WPM_NO_PRINTF
476
    	mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
LM's avatar
LM committed
477 478 479
#else
		if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
480 481 482 483 484 485 486 487 488
			}
            break;
        }
			
		case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
        {
            mavlink_waypoint_set_current_t wpc;
            mavlink_msg_waypoint_set_current_decode(msg, &wpc);
			
lm's avatar
lm committed
489
            if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/)
490 491 492 493 494 495 496
            {
                wpm.timestamp_lastaction = now;
				
                if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
                {
                    if (wpc.seq < wpm.size)
                    {
LM's avatar
LM committed
497
                        // if (verbose) // printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n");
498 499 500 501 502 503 504 505 506 507 508 509 510
                        wpm.current_active_wp_id = wpc.seq;
                        uint32_t i;
                        for(i = 0; i < wpm.size; i++)
                        {
                            if (i == wpm.current_active_wp_id)
                            {
                                wpm.waypoints[i].current = true;
                            }
                            else
                            {
                                wpm.waypoints[i].current = false;
                            }
                        }
LM's avatar
LM committed
511
#ifdef MAVLINK_WPM_NO_PRINTF
512
    	mavlink_missionlib_send_gcs_string("NEW WP SET");
LM's avatar
LM committed
513 514 515
#else
		if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm.current_active_wp_id);
#endif
516 517 518 519 520 521 522 523
                        wpm.yaw_reached = false;
                        wpm.pos_reached = false;
                        mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
                        mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
                        wpm.timestamp_firstinside_orbit = 0;
                    }
                    else
                    {
LM's avatar
LM committed
524
#ifdef MAVLINK_WPM_NO_PRINTF
525
    	mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
LM's avatar
LM committed
526 527 528
#else
		if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
#endif
529 530 531 532
                    }
                }
				else
				{
LM's avatar
LM committed
533
#ifdef MAVLINK_WPM_NO_PRINTF
534
    	mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
LM's avatar
LM committed
535 536 537
#else
		if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
#endif
538 539 540 541
				}
            }
			else
			{
LM's avatar
LM committed
542
#ifdef MAVLINK_WPM_NO_PRINTF
543
    	mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
LM's avatar
LM committed
544 545 546
#else
		if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
547 548 549 550 551 552 553 554
			}
            break;
        }
			
		case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
        {
            mavlink_waypoint_request_list_t wprl;
            mavlink_msg_waypoint_request_list_decode(msg, &wprl);
lm's avatar
lm committed
555
            if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/)
556 557 558 559 560 561 562
            {
                wpm.timestamp_lastaction = now;
				
                if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
                {
                    if (wpm.size > 0)
                    {
lm's avatar
lm committed
563 564
                        //if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
//                        if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
565 566 567 568 569 570 571
                        wpm.current_state = MAVLINK_WPM_STATE_SENDLIST;
                        wpm.current_wp_id = 0;
                        wpm.current_partner_sysid = msg->sysid;
                        wpm.current_partner_compid = msg->compid;
                    }
                    else
                    {
LM's avatar
LM committed
572
                        // if (verbose) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
573 574 575 576 577 578
                    }
                    wpm.current_count = wpm.size;
                    mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count);
                }
                else
                {
LM's avatar
LM committed
579
                    // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state);
580 581 582 583
                }
            }
			else
			{
LM's avatar
LM committed
584
				// if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n");
585 586 587 588 589 590 591 592 593
			}

            break;
        }
			
		case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
        {
            mavlink_waypoint_request_t wpr;
            mavlink_msg_waypoint_request_decode(msg, &wpr);
lm's avatar
lm committed
594
            if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/)
595 596 597 598 599 600
            {
                wpm.timestamp_lastaction = now;
				
                //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
                if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size))
                {
LM's avatar
LM committed
601 602 603
                    if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
                    {
#ifdef MAVLINK_WPM_NO_PRINTF
604
    	mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND");
LM's avatar
LM committed
605 606 607 608 609 610 611
#else
		if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
#endif
                    }
                    if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1)
                    {
#ifdef MAVLINK_WPM_NO_PRINTF
612
    	mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ");
LM's avatar
LM committed
613 614 615 616 617 618 619
#else
		if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
#endif
                    }
                    if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id)
                    {
#ifdef MAVLINK_WPM_NO_PRINTF
620
    	mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ");
LM's avatar
LM committed
621 622 623 624
#else
		if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
#endif
                    }
625 626 627 628 629 630 631
					
                    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);
                }
                else
                {
LM's avatar
LM committed
632
                    // if (verbose)
633
                    {
LM's avatar
LM committed
634 635 636
                        if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS))
						{
#ifdef MAVLINK_WPM_NO_PRINTF
637
    	mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
LM's avatar
LM committed
638 639 640 641 642
#else
		if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state);
#endif
		break;
						}
643 644
                        else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
                        {
LM's avatar
LM committed
645 646 647
                            if (wpr.seq != 0)
							{
#ifdef MAVLINK_WPM_NO_PRINTF
648
    	mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
LM's avatar
LM committed
649 650 651 652
#else
		if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
#endif
							}
653 654 655
                        }
                        else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)
                        {
LM's avatar
LM committed
656 657 658
                            if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1)
							{
#ifdef MAVLINK_WPM_NO_PRINTF
659
    	mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
LM's avatar
LM committed
660 661 662 663 664 665 666
#else
		if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1);
#endif
							}
							else if (wpr.seq >= wpm.size)
							{
#ifdef MAVLINK_WPM_NO_PRINTF
667
    	mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
LM's avatar
LM committed
668 669 670 671
#else
		if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
#endif
							}
672
                        }
LM's avatar
LM committed
673 674 675
                        else
						{
#ifdef MAVLINK_WPM_NO_PRINTF
676
    	mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?");
LM's avatar
LM committed
677 678 679 680
#else
		if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n");
#endif
						}
681 682 683 684 685 686
                    }
                }
            }
            else
            {
                //we we're target but already communicating with someone else
lm's avatar
lm committed
687
                if((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid))
688
                {
LM's avatar
LM committed
689
#ifdef MAVLINK_WPM_NO_PRINTF
690
    	mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
LM's avatar
LM committed
691 692 693
#else
		if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid);
#endif
694 695 696
                }
				else
				{
LM's avatar
LM committed
697
#ifdef MAVLINK_WPM_NO_PRINTF
698
    	mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
LM's avatar
LM committed
699 700 701
#else
		if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
702 703 704 705 706 707 708 709 710 711
				}

            }
            break;
        }
			
		case MAVLINK_MSG_ID_WAYPOINT_COUNT:
        {
            mavlink_waypoint_count_t wpc;
            mavlink_msg_waypoint_count_decode(msg, &wpc);
lm's avatar
lm committed
712
            if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/)
713 714 715 716 717 718 719
            {
                wpm.timestamp_lastaction = now;
				
                if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0))
                {
                    if (wpc.count > 0)
                    {
LM's avatar
LM committed
720 721 722
                        if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
                        {
#ifdef MAVLINK_WPM_NO_PRINTF
723
    	mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST");
LM's avatar
LM committed
724 725 726 727 728 729 730
#else
		if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
#endif
                        }
                        if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST)
                        {
#ifdef MAVLINK_WPM_NO_PRINTF
731
    	mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
LM's avatar
LM committed
732 733 734 735
#else
		if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
#endif
                        }
736 737 738 739 740 741 742
						
                        wpm.current_state = MAVLINK_WPM_STATE_GETLIST;
                        wpm.current_wp_id = 0;
                        wpm.current_partner_sysid = msg->sysid;
                        wpm.current_partner_compid = msg->compid;
                        wpm.current_count = wpc.count;
						
LM's avatar
LM committed
743
#ifdef MAVLINK_WPM_NO_PRINTF
744
    	mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY");
LM's avatar
LM committed
745 746 747
#else
		if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n");
#endif
748 749 750 751 752 753 754 755 756 757 758
						wpm.rcv_size = 0;
                        //while(waypoints_receive_buffer->size() > 0)
//                        {
//                            delete waypoints_receive_buffer->back();
//                            waypoints_receive_buffer->pop_back();
//                        }
						
                        mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id);
                    }
                    else if (wpc.count == 0)
                    {
LM's avatar
LM committed
759
#ifdef MAVLINK_WPM_NO_PRINTF
760
    	mavlink_missionlib_send_gcs_string("COUNT 0");
LM's avatar
LM committed
761 762 763
#else
		if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n");
#endif
764 765 766 767 768 769 770 771 772 773 774 775 776 777
						wpm.rcv_size = 0;
                        //while(waypoints_receive_buffer->size() > 0)
//                        {
//                            delete waypoints->back();
//                            waypoints->pop_back();
//                        }
                        wpm.current_active_wp_id = -1;
                        wpm.yaw_reached = false;
                        wpm.pos_reached = false;
                        break;
						
                    }
                    else
                    {
LM's avatar
LM committed
778
#ifdef MAVLINK_WPM_NO_PRINTF
779
    	mavlink_missionlib_send_gcs_string("IGN WP CMD");
LM's avatar
LM committed
780 781 782
#else
		if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
#endif
783 784 785 786
                    }
                }
                else
                {
LM's avatar
LM committed
787 788 789
                    if (!(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST))
					{
#ifdef MAVLINK_WPM_NO_PRINTF
790
    	mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
LM's avatar
LM committed
791 792 793 794 795 796 797
#else
		if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state);
#endif
					}
                    else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0)
					{
#ifdef MAVLINK_WPM_NO_PRINTF
798
    	mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
LM's avatar
LM committed
799 800 801 802 803 804 805
#else
		if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id);
#endif
					}
                    else
					{
#ifdef MAVLINK_WPM_NO_PRINTF
806
    	mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?");
LM's avatar
LM committed
807 808 809 810
#else
		if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
#endif
					}
811 812 813 814
                }
            }
			else
			{
LM's avatar
LM committed
815
#ifdef MAVLINK_WPM_NO_PRINTF
816
    	mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
LM's avatar
LM committed
817 818 819
#else
		if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
820 821 822 823 824 825 826 827 828 829
			}
            
        }
			break;
			
		case MAVLINK_MSG_ID_WAYPOINT:
        {
            mavlink_waypoint_t wp;
            mavlink_msg_waypoint_decode(msg, &wp);
			
830
			mavlink_missionlib_send_gcs_string("GOT WP");
831
			
lm's avatar
lm committed
832
            if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/))
833 834 835 836 837 838
            {
                wpm.timestamp_lastaction = now;
				
                //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
                if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count))
                {
lm's avatar
lm committed
839 840 841 842
//                    if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
//                    if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid);
//                    if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid);
//					
843 844 845
                    wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
                    mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]);
                    memcpy(newwp, &wp, sizeof(mavlink_waypoint_t));
lm's avatar
lm committed
846
					wpm.current_wp_id = wp.seq + 1;
847
					
LM's avatar
LM committed
848
                    // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
849 850 851
					
                    if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)
                    {
852
						mavlink_missionlib_send_gcs_string("GOT ALL WPS");
LM's avatar
LM committed
853
                        // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count);
854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876
						
                        mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
						
                        if (wpm.current_active_wp_id > wpm.rcv_size-1)
                        {
                            wpm.current_active_wp_id = wpm.rcv_size-1;
                        }
						
                        // switch the waypoints list
						// FIXME CHECK!!!
						for (int i = 0; i < wpm.current_count; ++i)
						{
							wpm.waypoints[i] = wpm.rcv_waypoints[i];
						}
						wpm.size = wpm.current_count;
						
                        //get the new current waypoint
                        uint32_t i;
                        for(i = 0; i < wpm.size; i++)
                        {
                            if (wpm.waypoints[i].current == 1)
                            {
                                wpm.current_active_wp_id = i;
LM's avatar
LM committed
877
                                //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id);
878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907
                                wpm.yaw_reached = false;
                                wpm.pos_reached = false;
								mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
                                mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
								wpm.timestamp_firstinside_orbit = 0;
                                break;
                            }
                        }
						
                        if (i == wpm.size)
                        {
                            wpm.current_active_wp_id = -1;
                            wpm.yaw_reached = false;
                            wpm.pos_reached = false;
                            wpm.timestamp_firstinside_orbit = 0;
                        }
						
                        wpm.current_state = MAVLINK_WPM_STATE_IDLE;
                    }
                    else
                    {
                        mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id);
                    }
                }
                else
                {
                    if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
                    {
                        //we're done receiving waypoints, answer with ack.
                        mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
LM's avatar
LM committed
908
                        // printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
909
                    }
LM's avatar
LM committed
910
                    // if (verbose)
911
                    {
LM's avatar
LM committed
912 913 914 915 916
                        if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS))
						{
							// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state);
							break;
						}
917 918
                        else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST)
                        {
LM's avatar
LM committed
919 920 921 922 923 924 925 926
                            if(!(wp.seq == 0))
							{
								// printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq);
							}
                            else
							{
								// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
							}
927 928 929
                        }
                        else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)
                        {
LM's avatar
LM committed
930 931 932 933 934 935 936 937 938 939 940 941
                            if (!(wp.seq == wpm.current_wp_id))
							{
								// printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id);
							}
                            else if (!(wp.seq < wpm.current_count))
							{
								// printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq);
							}
                            else
							{
								// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
							}
942
                        }
LM's avatar
LM committed
943 944 945 946
                        else
						{
							// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
						}
947 948 949 950 951 952
                    }
                }
            }
            else
            {
                //we we're target but already communicating with someone else
lm's avatar
lm committed
953
                if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
954
                {
LM's avatar
LM committed
955
                    // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid);
956
                }
lm's avatar
lm committed
957
                else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/)
958
                {
LM's avatar
LM committed
959
                    // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
960 961 962 963 964 965 966 967 968 969
                }
            }
            break;
        }
			
		case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
        {
            mavlink_waypoint_clear_all_t wpca;
            mavlink_msg_waypoint_clear_all_decode(msg, &wpca);
			
lm's avatar
lm committed
970
            if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE)
971 972 973
            {
                wpm.timestamp_lastaction = now;
				
LM's avatar
LM committed
974
                // if (verbose) // printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
975 976 977 978 979 980
                // Delete all waypoints
				wpm.size = 0;
                wpm.current_active_wp_id = -1;
                wpm.yaw_reached = false;
                wpm.pos_reached = false;
            }
lm's avatar
lm committed
981
            else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
982
            {
LM's avatar
LM committed
983
                // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state);
984 985 986 987 988 989
            }
            break;
        }
			
		default:
        {
LM's avatar
LM committed
990
            // if (debug) // printf("Waypoint: received message of unknown type");
991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004
            break;
        }
    }
	
    //check if the current waypoint was reached
    if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle)
    {
        if (wpm.current_active_wp_id < wpm.size)
        {
            mavlink_waypoint_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]);
			
            if (wpm.timestamp_firstinside_orbit == 0)
            {
                // Announce that last waypoint was reached
LM's avatar
LM committed
1005
                // if (verbose) // printf("*** Reached waypoint %u ***\n", cur_wp->seq);
1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034 1035
                mavlink_wpm_send_waypoint_reached(cur_wp->seq);
                wpm.timestamp_firstinside_orbit = now;
            }
			
            // check if the MAV was long enough inside the waypoint orbit
            //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
            if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000)
            {
                if (cur_wp->autocontinue)
                {
                    cur_wp->current = 0;
                    if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1)
                    {
                        //the last waypoint was reached, if auto continue is
                        //activated restart the waypoint list from the beginning
                        wpm.current_active_wp_id = 1;
                    }
                    else
                    {
                        if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size)
                            wpm.current_active_wp_id++;
                    }
					
                    // Fly to next waypoint
                    wpm.timestamp_firstinside_orbit = 0;
                    mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
                    mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
                    wpm.waypoints[wpm.current_active_wp_id].current = true;
                    wpm.pos_reached = false;
                    wpm.yaw_reached = false;
LM's avatar
LM committed
1036
                    // if (verbose) // printf("Set new waypoint (%u)\n", wpm.current_active_wp_id);
1037 1038 1039 1040 1041 1042 1043 1044 1045 1046
                }
            }
        }
    }
    else
    {
        wpm.timestamp_lastoutside_orbit = now;
    }
}