void mission_set_new_current_waypoint(void)
{
mavlink_mission_set_current_t mmst;
mavlink_msg_mission_set_current_decode(&received_msg, &mmst);
waypoint_t *wp;
/* Clear the old current waypoint flag */
wp = get_waypoint(waypoint_info.waypoint_list, waypoint_info.current_waypoint.number);
wp->data.current = 0;
/* Getting the seq of current waypoint */
/* Ming change : I think should set this too*/
waypoint_info.current_waypoint.number = mmst.seq;
Nav_update_current_wp_id(mmst.seq);
/* Set the new waypoint flag */
wp = get_waypoint(waypoint_info.waypoint_list, waypoint_info.current_waypoint.number);
wp->data.current = 1;
/* Send back the current waypoint seq as ack message */
mavlink_msg_mission_current_pack(1, 0, &msg, waypoint_info.current_waypoint.number);
send_package(&msg);
}
bool
OrderedTaskPoint::equals(const OrderedTaskPoint* other) const
{
return (get_waypoint() == other->get_waypoint()) &&
GetType() == other->GetType() &&
get_oz()->equals(other->get_oz()) &&
other->get_oz()->equals(get_oz());
}
OrderedTaskPoint*
OrderedTaskPoint::clone(const TaskBehaviour &task_behaviour,
const OrderedTaskBehaviour &ordered_task_behaviour,
const Waypoint* waypoint) const
{
if (waypoint == NULL)
waypoint = &get_waypoint();
switch (GetType()) {
case START:
return new StartPoint(get_oz()->clone(&waypoint->Location),
*waypoint, task_behaviour,
ordered_task_behaviour);
case AST:
return new ASTPoint(get_oz()->clone(&waypoint->Location),
*waypoint, task_behaviour,
ordered_task_behaviour);
case AAT:
return new AATPoint(get_oz()->clone(&waypoint->Location),
*waypoint, task_behaviour,
ordered_task_behaviour);
case FINISH:
return new FinishPoint(get_oz()->clone(&waypoint->Location),
*waypoint, task_behaviour,
ordered_task_behaviour);
case UNORDERED:
case ROUTE:
/* an OrderedTaskPoint must never be UNORDERED or ROUTE */
assert(false);
break;
}
return NULL;
}
void mission_write_waypoint_list(void)
{
uint32_t start_time, cur_time;
waypoint_t *cur_wp = NULL;
waypoint_t *new_waypoint;
/* Getting the waypoint count */
int new_waypoint_list_count = mavlink_msg_mission_count_get_count(&received_msg);
waypoint_info.is_busy = true;
int i;
for(i = 0; i < new_waypoint_list_count; i++) {
/* Generate the mission_request message */
mavlink_msg_mission_request_pack(
1, 0, &msg, 255, 0, i /* waypoint index */
);
send_package(&msg);
/* Create a new node of waypoint */
if (waypoint_info.waypoint_count > i) {
new_waypoint = get_waypoint(waypoint_info.waypoint_list, i);
} else {
/* Create a new node of waypoint */
new_waypoint = create_waypoint_node();
}
start_time = get_system_time_ms();
/* Waiting for new message */
while(received_msg.msgid != 39) {
cur_time = get_system_time_ms();
//Suspend the task to read the new message
vTaskDelay(100);
/* Time out, leave */
if((cur_time - start_time) > TIMEOUT_CNT) {
return;
}
}
/* Get the waypoint message */
mavlink_msg_mission_item_decode(&received_msg, &(new_waypoint->data));
/* Clear the received message */
clear_message_id(&received_msg);
/* insert the new waypoint at the end of the list */
if(i == 0) {
//First node of the list
waypoint_info.waypoint_list = cur_wp = new_waypoint;
} else {
cur_wp->next = new_waypoint;
cur_wp = cur_wp->next;
}
}
/* Update the wayppoint, navigation manager */
waypoint_info.waypoint_count = new_waypoint_list_count;
waypoint_info.is_busy = false;
nav_waypoint_list_is_updated = false; /* From navigation point of view */
/* Send a mission ack Message at the end */
mavlink_msg_mission_ack_pack(1, 0, &msg, 255, 0, 0);
send_package(&msg);
}
