waypoints.c
Go to the documentation of this file.
00001 /*******************************************************************************
00002  
00003  Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch
00004  
00005  This program is free software: you can redistribute it and/or modify
00006  it under the terms of the GNU General Public License as published by
00007  the Free Software Foundation, either version 3 of the License, or
00008  (at your option) any later version.
00009  
00010  This program is distributed in the hope that it will be useful,
00011  but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  GNU General Public License for more details.
00014  
00015  You should have received a copy of the GNU General Public License
00016  along with this program.  If not, see <http://www.gnu.org/licenses/>.
00017  
00018  ****************************************************************************/
00019 
00020 #include "waypoints.h"
00021 #include <math.h>
00022 
00023 bool debug = true;
00024 bool verbose = true;
00025 
00026 extern mavlink_wpm_storage wpm;
00027 
00028 extern void mavlink_missionlib_send_message(mavlink_message_t* msg);
00029 extern void mavlink_missionlib_send_gcs_string(const char* string);
00030 extern uint64_t mavlink_missionlib_get_system_timestamp();
00031 extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
00032                 float param2, float param3, float param4, float param5_lat_x,
00033                 float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
00034 
00035 
00036 #define MAVLINK_WPM_NO_PRINTF
00037 
00038 uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
00039 
00040 void mavlink_wpm_init(mavlink_wpm_storage* state)
00041 {
00042         // Set all waypoints to zero
00043         
00044         // Set count to zero
00045         state->size = 0;
00046         state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
00047         state->current_state = MAVLINK_WPM_STATE_IDLE;
00048         state->current_partner_sysid = 0;
00049         state->current_partner_compid = 0;
00050         state->timestamp_lastaction = 0;
00051         state->timestamp_last_send_setpoint = 0;
00052         state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
00053         state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT;
00054         state->idle = false;                                    
00055         state->current_active_wp_id = -1;               
00056         state->yaw_reached = false;                                             
00057         state->pos_reached = false;                                             
00058         state->timestamp_lastoutside_orbit = 0;
00059         state->timestamp_firstinside_orbit = 0;
00060         
00061 }
00062 
00063 /*
00064  *  @brief Sends an waypoint ack message
00065  */
00066 void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
00067 {
00068     mavlink_message_t msg;
00069     mavlink_mission_ack_t wpa;
00070         
00071     wpa.target_system = wpm.current_partner_sysid;
00072     wpa.target_component = wpm.current_partner_compid;
00073     wpa.type = type;
00074         
00075     mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa);
00076     mavlink_missionlib_send_message(&msg);
00077         
00078     // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
00079         
00080     if (MAVLINK_WPM_TEXT_FEEDBACK)
00081         {
00082 #ifdef MAVLINK_WPM_NO_PRINTF
00083         mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
00084 #else
00085                 if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
00086 #endif
00087                 mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
00088         }
00089 }
00090 
00091 /*
00092  *  @brief Broadcasts the new target waypoint and directs the MAV to fly there
00093  *
00094  *  This function broadcasts its new active waypoint sequence number and
00095  *  sends a message to the controller, advising it to fly to the coordinates
00096  *  of the waypoint with a given orientation
00097  *
00098  *  @param seq The waypoint sequence number the MAV should fly to.
00099  */
00100 void mavlink_wpm_send_waypoint_current(uint16_t seq)
00101 {
00102     if(seq < wpm.size)
00103     {
00104         mavlink_mission_item_t *cur = &(wpm.waypoints[seq]);
00105                 
00106         mavlink_message_t msg;
00107         mavlink_mission_current_t wpc;
00108                 
00109         wpc.seq = cur->seq;
00110                 
00111         mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
00112         mavlink_missionlib_send_message(&msg);
00113                 
00114         // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
00115                 
00116         if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Set current waypoint\n"); 
00117     }
00118     else
00119     {
00120         if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds\n");
00121     }
00122 }
00123 
00124 /*
00125  *  @brief Directs the MAV to fly to a position
00126  *
00127  *  Sends a message to the controller, advising it to fly to the coordinates
00128  *  of the waypoint with a given orientation
00129  *
00130  *  @param seq The waypoint sequence number the MAV should fly to.
00131  */
00132 void mavlink_wpm_send_setpoint(uint16_t seq)
00133 {
00134     if(seq < wpm.size)
00135     {
00136         mavlink_mission_item_t *cur = &(wpm.waypoints[seq]);
00137         mavlink_missionlib_current_waypoint_changed(cur->seq, cur->param1,
00138                         cur->param2, cur->param3, cur->param4, cur->x,
00139                         cur->y, cur->z, cur->frame, cur->command);
00140 
00141         wpm.timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp();
00142     }
00143     else
00144     {
00145         if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); 
00146     }
00147 }
00148 
00149 void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count)
00150 {
00151     mavlink_message_t msg;
00152     mavlink_mission_count_t wpc;
00153         
00154     wpc.target_system = wpm.current_partner_sysid;
00155     wpc.target_component = wpm.current_partner_compid;
00156     wpc.count = count;
00157         
00158     mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
00159     mavlink_missionlib_send_message(&msg);
00160         
00161     if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint count"); 
00162         
00163     // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
00164 }
00165 
00166 void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
00167 {
00168     if (seq < wpm.size)
00169     {
00170         mavlink_message_t msg;
00171         mavlink_mission_item_t *wp = &(wpm.waypoints[seq]);
00172         wp->target_system = wpm.current_partner_sysid;
00173         wp->target_component = wpm.current_partner_compid;
00174         mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp);
00175         mavlink_missionlib_send_message(&msg);
00176         if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); 
00177                 
00178         // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
00179     }
00180     else
00181     {
00182         if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n");
00183     }
00184 }
00185 
00186 void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
00187 {
00188     if (seq < wpm.max_size)
00189     {
00190         mavlink_message_t msg;
00191         mavlink_mission_request_t wpr;
00192         wpr.target_system = wpm.current_partner_sysid;
00193         wpr.target_component = wpm.current_partner_compid;
00194         wpr.seq = seq;
00195         mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr);
00196         mavlink_missionlib_send_message(&msg);
00197         if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint request"); 
00198                 
00199         // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
00200     }
00201     else
00202     {
00203         if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n");
00204     }
00205 }
00206 
00207 /*
00208  *  @brief emits a message that a waypoint reached
00209  *
00210  *  This function broadcasts a message that a waypoint is reached.
00211  *
00212  *  @param seq The waypoint sequence number the MAV has reached.
00213  */
00214 void mavlink_wpm_send_waypoint_reached(uint16_t seq)
00215 {
00216     mavlink_message_t msg;
00217     mavlink_mission_item_reached_t wp_reached;
00218         
00219     wp_reached.seq = seq;
00220         
00221     mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached);
00222     mavlink_missionlib_send_message(&msg);
00223         
00224     if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint reached message"); 
00225         
00226     // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
00227 }
00228 
00229 //float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z)
00230 //{
00231 //    if (seq < wpm.size)
00232 //    {
00233 //        mavlink_mission_item_t *cur = waypoints->at(seq);
00234 //              
00235 //        const PxVector3 A(cur->x, cur->y, cur->z);
00236 //        const PxVector3 C(x, y, z);
00237 //              
00238 //        // seq not the second last waypoint
00239 //        if ((uint16_t)(seq+1) < wpm.size)
00240 //        {
00241 //            mavlink_mission_item_t *next = waypoints->at(seq+1);
00242 //            const PxVector3 B(next->x, next->y, next->z);
00243 //            const float r = (B-A).dot(C-A) / (B-A).lengthSquared();
00244 //            if (r >= 0 && r <= 1)
00245 //            {
00246 //                const PxVector3 P(A + r*(B-A));
00247 //                return (P-C).length();
00248 //            }
00249 //            else if (r < 0.f)
00250 //            {
00251 //                return (C-A).length();
00252 //            }
00253 //            else
00254 //            {
00255 //                return (C-B).length();
00256 //            }
00257 //        }
00258 //        else
00259 //        {
00260 //            return (C-A).length();
00261 //        }
00262 //    }
00263 //    else
00264 //    {
00265 //        // if (verbose) // printf("ERROR: index out of bounds\n");
00266 //    }
00267 //    return -1.f;
00268 //}
00269 
00270 float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z)
00271 {
00272 //    if (seq < wpm.size)
00273 //    {
00274 //        mavlink_mission_item_t *cur = waypoints->at(seq);
00275 //              
00276 //        const PxVector3 A(cur->x, cur->y, cur->z);
00277 //        const PxVector3 C(x, y, z);
00278 //              
00279 //        return (C-A).length();
00280 //    }
00281 //    else
00282 //    {
00283 //        // if (verbose) // printf("ERROR: index out of bounds\n");
00284 //    }
00285     return -1.f;
00286 }
00287 
00288 void mavlink_wpm_loop()
00289 {
00290     //check for timed-out operations
00291     uint64_t now = mavlink_missionlib_get_system_timestamp();
00292     if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
00293     {
00294 #ifdef MAVLINK_WPM_NO_PRINTF
00295         mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE");
00296 #else
00297                 if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state);
00298 #endif
00299                 wpm.current_state = MAVLINK_WPM_STATE_IDLE;
00300         wpm.current_count = 0;
00301         wpm.current_partner_sysid = 0;
00302         wpm.current_partner_compid = 0;
00303         wpm.current_wp_id = -1;
00304                 
00305         if(wpm.size == 0)
00306         {
00307             wpm.current_active_wp_id = -1;
00308         }
00309     }
00310         
00311     if(now-wpm.timestamp_last_send_setpoint > wpm.delay_setpoint && wpm.current_active_wp_id < wpm.size)
00312     {
00313         mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
00314     }
00315 }
00316 
00317 void mavlink_wpm_message_handler(const mavlink_message_t* msg)
00318 {
00319         uint64_t now = mavlink_missionlib_get_system_timestamp();
00320     switch(msg->msgid)
00321     {
00322                 case MAVLINK_MSG_ID_ATTITUDE:
00323         {
00324             if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size)
00325             {
00326                 mavlink_mission_item_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]);
00327                 if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED)
00328                 {
00329                     mavlink_attitude_t att;
00330                     mavlink_msg_attitude_decode(msg, &att);
00331                     float yaw_tolerance = wpm.accept_range_yaw;
00332                     //compare current yaw
00333                     if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI)
00334                     {
00335                         if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
00336                             wpm.yaw_reached = true;
00337                     }
00338                     else if(att.yaw - yaw_tolerance < 0.0f)
00339                     {
00340                         float lowerBound = 360.0f + att.yaw - yaw_tolerance;
00341                         if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
00342                             wpm.yaw_reached = true;
00343                     }
00344                     else
00345                     {
00346                         float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI;
00347                         if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
00348                             wpm.yaw_reached = true;
00349                     }
00350                 }
00351             }
00352             break;
00353         }
00354                         
00355                 case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
00356         {
00357             if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size)
00358             {
00359                 mavlink_mission_item_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]);
00360                                 
00361                 if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED)
00362                 {
00363                     mavlink_local_position_ned_t pos;
00364                     mavlink_msg_local_position_ned_decode(msg, &pos);
00366                                         
00367                     wpm.pos_reached = false;
00368                                         
00369                     // compare current position (given in message) with current waypoint
00370                     float orbit = wp->param1;
00371                                         
00372                     float dist;
00373 //                    if (wp->param2 == 0)
00374 //                    {
00375 //                                              // FIXME segment distance
00376 //                        //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z);
00377 //                    }
00378 //                    else
00379 //                    {
00380                         dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z);
00381 //                    }
00382                                         
00383                     if (dist >= 0.f && dist <= orbit && wpm.yaw_reached)
00384                     {
00385                         wpm.pos_reached = true;
00386                     }
00387                 }
00388             }
00389             break;
00390         }
00391                         
00392 //              case MAVLINK_MSG_ID_CMD: // special action from ground station
00393 //        {
00394 //            mavlink_cmd_t action;
00395 //            mavlink_msg_cmd_decode(msg, &action);
00396 //            if(action.target == mavlink_system.sysid)
00397 //            {
00398 //                // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
00399 //                switch (action.action)
00400 //                {
00401 //                                              //                              case MAV_ACTION_LAUNCH:
00402 //                                              //                                      // if (verbose) std::cerr << "Launch received" << std::endl;
00403 //                                              //                                      current_active_wp_id = 0;
00404 //                                              //                                      if (wpm.size>0)
00405 //                                              //                                      {
00406 //                                              //                                              setActive(waypoints[current_active_wp_id]);
00407 //                                              //                                      }
00408 //                                              //                                      else
00409 //                                              //                                              // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
00410 //                                              //                                      break;
00411 //                                              
00412 //                                              //                              case MAV_ACTION_CONTINUE:
00413 //                                              //                                      // if (verbose) std::c
00414 //                                              //                                      err << "Continue received" << std::endl;
00415 //                                              //                                      idle = false;
00416 //                                              //                                      setActive(waypoints[current_active_wp_id]);
00417 //                                              //                                      break;
00418 //                                              
00419 //                                              //                              case MAV_ACTION_HALT:
00420 //                                              //                                      // if (verbose) std::cerr << "Halt received" << std::endl;
00421 //                                              //                                      idle = true;
00422 //                                              //                                      break;
00423 //                                              
00424 //                                              //                              default:
00425 //                                              //                                      // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
00426 //                                              //                                      break;
00427 //                }
00428 //            }
00429 //            break;
00430 //        }
00431                         
00432                 case MAVLINK_MSG_ID_MISSION_ACK:
00433         {
00434             mavlink_mission_ack_t wpa;
00435             mavlink_msg_mission_ack_decode(msg, &wpa);
00436                         
00437             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*/))
00438             {
00439                 wpm.timestamp_lastaction = now;
00440                                 
00441                 if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)
00442                 {
00443                     if (wpm.current_wp_id == wpm.size-1)
00444                     {
00445 #ifdef MAVLINK_WPM_NO_PRINTF
00446         mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
00447 #else
00448                 if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
00449 #endif
00450                                                 wpm.current_state = MAVLINK_WPM_STATE_IDLE;
00451                         wpm.current_wp_id = 0;
00452                     }
00453                 }
00454             }
00455                         else
00456                         {
00457 #ifdef MAVLINK_WPM_NO_PRINTF
00458         mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
00459 #else
00460                 if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
00461 #endif
00462                         }
00463             break;
00464         }
00465                         
00466                 case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
00467         {
00468             mavlink_mission_set_current_t wpc;
00469             mavlink_msg_mission_set_current_decode(msg, &wpc);
00470                         
00471             if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/)
00472             {
00473                 wpm.timestamp_lastaction = now;
00474                                 
00475                 if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
00476                 {
00477                     if (wpc.seq < wpm.size)
00478                     {
00479                         // if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n");
00480                         wpm.current_active_wp_id = wpc.seq;
00481                         uint32_t i;
00482                         for(i = 0; i < wpm.size; i++)
00483                         {
00484                             if (i == wpm.current_active_wp_id)
00485                             {
00486                                 wpm.waypoints[i].current = true;
00487                             }
00488                             else
00489                             {
00490                                 wpm.waypoints[i].current = false;
00491                             }
00492                         }
00493 #ifdef MAVLINK_WPM_NO_PRINTF
00494         mavlink_missionlib_send_gcs_string("NEW WP SET");
00495 #else
00496                 if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm.current_active_wp_id);
00497 #endif
00498                         wpm.yaw_reached = false;
00499                         wpm.pos_reached = false;
00500                         mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
00501                         mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
00502                         wpm.timestamp_firstinside_orbit = 0;
00503                     }
00504                     else
00505                     {
00506 #ifdef MAVLINK_WPM_NO_PRINTF
00507         mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
00508 #else
00509                 if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n");
00510 #endif
00511                     }
00512                 }
00513                                 else
00514                                 {
00515 #ifdef MAVLINK_WPM_NO_PRINTF
00516         mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
00517 #else
00518                 if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
00519 #endif
00520                                 }
00521             }
00522                         else
00523                         {
00524 #ifdef MAVLINK_WPM_NO_PRINTF
00525         mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
00526 #else
00527                 if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
00528 #endif
00529                         }
00530             break;
00531         }
00532                         
00533                 case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
00534         {
00535             mavlink_mission_request_list_t wprl;
00536             mavlink_msg_mission_request_list_decode(msg, &wprl);
00537             if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/)
00538             {
00539                 wpm.timestamp_lastaction = now;
00540                                 
00541                 if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
00542                 {
00543                     if (wpm.size > 0)
00544                     {
00545                         //if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
00546 //                        if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
00547                         wpm.current_state = MAVLINK_WPM_STATE_SENDLIST;
00548                         wpm.current_wp_id = 0;
00549                         wpm.current_partner_sysid = msg->sysid;
00550                         wpm.current_partner_compid = msg->compid;
00551                     }
00552                     else
00553                     {
00554                         // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
00555                     }
00556                     wpm.current_count = wpm.size;
00557                     mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count);
00558                 }
00559                 else
00560                 {
00561                     // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state);
00562                 }
00563             }
00564                         else
00565                         {
00566                                 // if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n");
00567                         }
00568 
00569             break;
00570         }
00571                         
00572                 case MAVLINK_MSG_ID_MISSION_REQUEST:
00573         {
00574             mavlink_mission_request_t wpr;
00575             mavlink_msg_mission_request_decode(msg, &wpr);
00576             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*/)
00577             {
00578                 wpm.timestamp_lastaction = now;
00579                                 
00580                 //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)
00581                 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))
00582                 {
00583                     if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
00584                     {
00585 #ifdef MAVLINK_WPM_NO_PRINTF
00586         mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND");
00587 #else
00588                 if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
00589 #endif
00590                     }
00591                     if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1)
00592                     {
00593 #ifdef MAVLINK_WPM_NO_PRINTF
00594         mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ");
00595 #else
00596                 if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
00597 #endif
00598                     }
00599                     if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id)
00600                     {
00601 #ifdef MAVLINK_WPM_NO_PRINTF
00602         mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ");
00603 #else
00604                 if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
00605 #endif
00606                     }
00607                                         
00608                     wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
00609                     wpm.current_wp_id = wpr.seq;
00610                     mavlink_wpm_send_waypoint(wpm.current_partner_sysid, wpm.current_partner_compid, wpr.seq);
00611                 }
00612                 else
00613                 {
00614                     // if (verbose)
00615                     {
00616                         if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS))
00617                                                 {
00618 #ifdef MAVLINK_WPM_NO_PRINTF
00619         mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
00620 #else
00621                 if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state);
00622 #endif
00623                 break;
00624                                                 }
00625                         else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
00626                         {
00627                             if (wpr.seq != 0)
00628                                                         {
00629 #ifdef MAVLINK_WPM_NO_PRINTF
00630         mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
00631 #else
00632                 if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
00633 #endif
00634                                                         }
00635                         }
00636                         else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)
00637                         {
00638                             if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1)
00639                                                         {
00640 #ifdef MAVLINK_WPM_NO_PRINTF
00641         mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
00642 #else
00643                 if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_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);
00644 #endif
00645                                                         }
00646                                                         else if (wpr.seq >= wpm.size)
00647                                                         {
00648 #ifdef MAVLINK_WPM_NO_PRINTF
00649         mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
00650 #else
00651                 if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
00652 #endif
00653                                                         }
00654                         }
00655                         else
00656                                                 {
00657 #ifdef MAVLINK_WPM_NO_PRINTF
00658         mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?");
00659 #else
00660                 if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n");
00661 #endif
00662                                                 }
00663                     }
00664                 }
00665             }
00666             else
00667             {
00668                 //we we're target but already communicating with someone else
00669                 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))
00670                 {
00671 #ifdef MAVLINK_WPM_NO_PRINTF
00672         mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
00673 #else
00674                 if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid);
00675 #endif
00676                 }
00677                                 else
00678                                 {
00679 #ifdef MAVLINK_WPM_NO_PRINTF
00680         mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
00681 #else
00682                 if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
00683 #endif
00684                                 }
00685 
00686             }
00687             break;
00688         }
00689                         
00690                 case MAVLINK_MSG_ID_MISSION_COUNT:
00691         {
00692             mavlink_mission_count_t wpc;
00693             mavlink_msg_mission_count_decode(msg, &wpc);
00694             if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/)
00695             {
00696                 wpm.timestamp_lastaction = now;
00697                                 
00698                 if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0))
00699                 {
00700                     if (wpc.count > 0)
00701                     {
00702                         if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
00703                         {
00704 #ifdef MAVLINK_WPM_NO_PRINTF
00705         mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST");
00706 #else
00707                 if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
00708 #endif
00709                         }
00710                         if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST)
00711                         {
00712 #ifdef MAVLINK_WPM_NO_PRINTF
00713         mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
00714 #else
00715                 if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
00716 #endif
00717                         }
00718                                                 
00719                         wpm.current_state = MAVLINK_WPM_STATE_GETLIST;
00720                         wpm.current_wp_id = 0;
00721                         wpm.current_partner_sysid = msg->sysid;
00722                         wpm.current_partner_compid = msg->compid;
00723                         wpm.current_count = wpc.count;
00724                                                 
00725 #ifdef MAVLINK_WPM_NO_PRINTF
00726         mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY");
00727 #else
00728                 if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n");
00729 #endif
00730                                                 wpm.rcv_size = 0;
00731                         //while(waypoints_receive_buffer->size() > 0)
00732 //                        {
00733 //                            delete waypoints_receive_buffer->back();
00734 //                            waypoints_receive_buffer->pop_back();
00735 //                        }
00736                                                 
00737                         mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id);
00738                     }
00739                     else if (wpc.count == 0)
00740                     {
00741 #ifdef MAVLINK_WPM_NO_PRINTF
00742         mavlink_missionlib_send_gcs_string("COUNT 0");
00743 #else
00744                 if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n");
00745 #endif
00746                                                 wpm.rcv_size = 0;
00747                         //while(waypoints_receive_buffer->size() > 0)
00748 //                        {
00749 //                            delete waypoints->back();
00750 //                            waypoints->pop_back();
00751 //                        }
00752                         wpm.current_active_wp_id = -1;
00753                         wpm.yaw_reached = false;
00754                         wpm.pos_reached = false;
00755                         break;
00756                                                 
00757                     }
00758                     else
00759                     {
00760 #ifdef MAVLINK_WPM_NO_PRINTF
00761         mavlink_missionlib_send_gcs_string("IGN WP CMD");
00762 #else
00763                 if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
00764 #endif
00765                     }
00766                 }
00767                 else
00768                 {
00769                     if (!(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST))
00770                                         {
00771 #ifdef MAVLINK_WPM_NO_PRINTF
00772         mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
00773 #else
00774                 if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state);
00775 #endif
00776                                         }
00777                     else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0)
00778                                         {
00779 #ifdef MAVLINK_WPM_NO_PRINTF
00780         mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
00781 #else
00782                 if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id);
00783 #endif
00784                                         }
00785                     else
00786                                         {
00787 #ifdef MAVLINK_WPM_NO_PRINTF
00788         mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?");
00789 #else
00790                 if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n");
00791 #endif
00792                                         }
00793                 }
00794             }
00795                         else
00796                         {
00797 #ifdef MAVLINK_WPM_NO_PRINTF
00798         mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
00799 #else
00800                 if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
00801 #endif
00802                         }
00803             
00804         }
00805                         break;
00806                         
00807                 case MAVLINK_MSG_ID_MISSION_ITEM:
00808         {
00809             mavlink_mission_item_t wp;
00810             mavlink_msg_mission_item_decode(msg, &wp);
00811                         
00812                         mavlink_missionlib_send_gcs_string("GOT WP");
00813                         
00814             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*/))
00815             {
00816                 wpm.timestamp_lastaction = now;
00817                                 
00818                 //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
00819                 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))
00820                 {
00821 //                    if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
00822 //                    if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid);
00823 //                    if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid);
00824 //                                      
00825                     wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
00826                     mavlink_mission_item_t* newwp = &(wpm.rcv_waypoints[wp.seq]);
00827                     memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
00828                                         wpm.current_wp_id = wp.seq + 1;
00829                                         
00830                     // 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);
00831                                         
00832                     if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)
00833                     {
00834                                                 mavlink_missionlib_send_gcs_string("GOT ALL WPS");
00835                         // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count);
00836                                                 
00837                         mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
00838                                                 
00839                         if (wpm.current_active_wp_id > wpm.rcv_size-1)
00840                         {
00841                             wpm.current_active_wp_id = wpm.rcv_size-1;
00842                         }
00843                                                 
00844                         // switch the waypoints list
00845                                                 // FIXME CHECK!!!
00846                                                 for (int i = 0; i < wpm.current_count; ++i)
00847                                                 {
00848                                                         wpm.waypoints[i] = wpm.rcv_waypoints[i];
00849                                                 }
00850                                                 wpm.size = wpm.current_count;
00851                                                 
00852                         //get the new current waypoint
00853                         uint32_t i;
00854                         for(i = 0; i < wpm.size; i++)
00855                         {
00856                             if (wpm.waypoints[i].current == 1)
00857                             {
00858                                 wpm.current_active_wp_id = i;
00860                                 wpm.yaw_reached = false;
00861                                 wpm.pos_reached = false;
00862                                                                 mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
00863                                 mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
00864                                                                 wpm.timestamp_firstinside_orbit = 0;
00865                                 break;
00866                             }
00867                         }
00868                                                 
00869                         if (i == wpm.size)
00870                         {
00871                             wpm.current_active_wp_id = -1;
00872                             wpm.yaw_reached = false;
00873                             wpm.pos_reached = false;
00874                             wpm.timestamp_firstinside_orbit = 0;
00875                         }
00876                                                 
00877                         wpm.current_state = MAVLINK_WPM_STATE_IDLE;
00878                     }
00879                     else
00880                     {
00881                         mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id);
00882                     }
00883                 }
00884                 else
00885                 {
00886                     if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
00887                     {
00888                         //we're done receiving waypoints, answer with ack.
00889                         mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
00890                         // printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
00891                     }
00892                     // if (verbose)
00893                     {
00894                         if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS))
00895                                                 {
00896                                                         // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state);
00897                                                         break;
00898                                                 }
00899                         else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST)
00900                         {
00901                             if(!(wp.seq == 0))
00902                                                         {
00903                                                                 // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq);
00904                                                         }
00905                             else
00906                                                         {
00907                                                                 // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
00908                                                         }
00909                         }
00910                         else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)
00911                         {
00912                             if (!(wp.seq == wpm.current_wp_id))
00913                                                         {
00914                                                                 // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id);
00915                                                         }
00916                             else if (!(wp.seq < wpm.current_count))
00917                                                         {
00918                                                                 // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq);
00919                                                         }
00920                             else
00921                                                         {
00922                                                                 // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
00923                                                         }
00924                         }
00925                         else
00926                                                 {
00927                                                         // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
00928                                                 }
00929                     }
00930                 }
00931             }
00932             else
00933             {
00934                 //we we're target but already communicating with someone else
00935                 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)
00936                 {
00937                     // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid);
00938                 }
00939                 else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/)
00940                 {
00941                     // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
00942                 }
00943             }
00944             break;
00945         }
00946                         
00947                 case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
00948         {
00949             mavlink_mission_clear_all_t wpca;
00950             mavlink_msg_mission_clear_all_decode(msg, &wpca);
00951                         
00952             if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE)
00953             {
00954                 wpm.timestamp_lastaction = now;
00955                                 
00956                 // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
00957                 // Delete all waypoints
00958                                 wpm.size = 0;
00959                 wpm.current_active_wp_id = -1;
00960                 wpm.yaw_reached = false;
00961                 wpm.pos_reached = false;
00962             }
00963             else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
00964             {
00965                 // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state);
00966             }
00967             break;
00968         }
00969                         
00970                 default:
00971         {
00972             // if (debug) // printf("Waypoint: received message of unknown type");
00973             break;
00974         }
00975     }
00976         
00977     //check if the current waypoint was reached
00978     if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle)
00979     {
00980         if (wpm.current_active_wp_id < wpm.size)
00981         {
00982             mavlink_mission_item_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]);
00983                         
00984             if (wpm.timestamp_firstinside_orbit == 0)
00985             {
00986                 // Announce that last waypoint was reached
00987                 // if (verbose) // printf("*** Reached waypoint %u ***\n", cur_wp->seq);
00988                 mavlink_wpm_send_waypoint_reached(cur_wp->seq);
00989                 wpm.timestamp_firstinside_orbit = now;
00990             }
00991                         
00992             // check if the MAV was long enough inside the waypoint orbit
00993             //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
00994             if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000)
00995             {
00996                 if (cur_wp->autocontinue)
00997                 {
00998                     cur_wp->current = 0;
00999                     if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1)
01000                     {
01001                         //the last waypoint was reached, if auto continue is
01002                         //activated restart the waypoint list from the beginning
01003                         wpm.current_active_wp_id = 1;
01004                     }
01005                     else
01006                     {
01007                         if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size)
01008                             wpm.current_active_wp_id++;
01009                     }
01010                                         
01011                     // Fly to next waypoint
01012                     wpm.timestamp_firstinside_orbit = 0;
01013                     mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
01014                     mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
01015                     wpm.waypoints[wpm.current_active_wp_id].current = true;
01016                     wpm.pos_reached = false;
01017                     wpm.yaw_reached = false;
01018                     // if (verbose) // printf("Set new waypoint (%u)\n", wpm.current_active_wp_id);
01019                 }
01020             }
01021         }
01022     }
01023     else
01024     {
01025         wpm.timestamp_lastoutside_orbit = now;
01026     }
01027 }
01028 


mavlink
Author(s): Lorenz Meier
autogenerated on Sun May 22 2016 04:05:43