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);
}
//to be called after MISSION_SET_CURRENT ( #41 ) OR after a mission item was reached by the rover.
// In the latter case set *msg = NULL
COMM_MAV_RESULT navigation_next_mission_item(COMM_MAV_MSG_TARGET *target, mavlink_message_t *msg, mavlink_message_t *answer_msg)
{
if(msg == NULL) // automatically go to next item in list
{
mission_items.current_index++;
if(mission_items.current_index<mission_items.count) //activate next target if available
{
mission_items.item[mission_items.current_index].current = 1;
#ifdef FLASH_ENABLED
log_write_mavlink_item_list(true, &pos_counter);
#endif
}
else
{
// pretend to start reaching home again but do not update target
mavlink_msg_mission_current_pack(mavlink_system.sysid, MAV_COMP_ID_MISSIONPLANNER, answer_msg,
0);
navigation_status.current_state = STOP;
// SPC gets disarmed automatically as long as it is sending heartbeats,
// because current target is 0 (HOME) again
return REPLY_TO_SENDER;
}
}
else //received command to change the current item
{
mission_items.item[mission_items.current_index].current = 0;
mission_items.current_index = mavlink_msg_mission_set_current_get_seq(msg);
if(mission_items.current_index < mission_items.count)
mission_items.item[mission_items.current_index].current = 1;
else
return NO_ANSWER; //TODO: reply NACK
#ifdef FLASH_ENABLED
log_write_mavlink_item_list(false, &pos_counter);
#endif
}
// Set state to reach new waypoints:
navigation_update_current_target();
// Inform controller about the new waypoint:
mavlink_msg_mission_current_pack(mavlink_system.sysid, MAV_COMP_ID_MISSIONPLANNER, answer_msg,
mission_items.current_index);
return REPLY_TO_SENDER;
}
/**
* Transmit the current mission index via UART1. GetCurrentMission returns a -1 if there're no missions,
* so we check and only transmit valid current missions.
*/
void MavLinkSendCurrentMission(void)
{
int8_t currentMission;
GetCurrentMission(¤tMission);
if (currentMission != -1) {
mavlink_message_t msg;
mavlink_msg_mission_current_pack(mavlink_system.sysid, mavlink_system.compid, &msg, (uint16_t)currentMission);
len = mavlink_msg_to_send_buffer(buf, &msg);
uart1EnqueueData(buf, (uint8_t)len);
}
}
static void send_current_waypoint(void)
{
if(waypoint_info.current_waypoint.is_update == true) {
mavlink_message_t msg;
/* Update the new current waypoint */
mavlink_msg_mission_current_pack(1, 0, &msg,
waypoint_info.current_waypoint.number);
send_package(&msg);
waypoint_info.current_waypoint.is_update = false;
}
}
uint16_t MissionInterfaceResponse(uint8_t system_id, uint8_t component_id){
mavlink_message_t msg;
uint16_t bytes2Send = 0;
uint8_t CopyMsgToBuff = 0;
char vr_message[50];
// if (mlPending.wpProtState == WP_PROT_TX_WP) { //SLUGS Nav DBG info
// memset(vr_message, 0, sizeof (vr_message));
// sprintf(vr_message, "%d: y =%2.2f x =%2.2f z =%2.2f o =%d t =%d", mlPending.wpCurrentWpInTransaction, (double)mlWpValues.lat[mlPending.wpCurrentWpInTransaction],
// (double)mlWpValues.lon[mlPending.wpCurrentWpInTransaction],
// (double)mlWpValues.alt[mlPending.wpCurrentWpInTransaction],
// mlWpValues.orbit[mlPending.wpCurrentWpInTransaction],
// mlWpValues.type[mlPending.wpCurrentWpInTransaction]);
// bytes2Send += sendQGCDebugMessage(vr_message, 0, UartOutBuff, bytes2Send + 1);
// }
//if (mlPending.wpProtState == WP_PROT_GETTING_WP_IDLE) { //SLUGS Nav DBG info
// memset(vr_message, 0, sizeof (vr_message));
// sprintf(vr_message, "com = %d, tb =%2.2f ta = %2.2f", sw_intTemp, (double)fl_temp1, (double)fl_temp2);
// bytes2Send += sendQGCDebugMessage(vr_message, 0, UartOutBuff, bytes2Send + 1);
//}
// Current mission item (1 off indexing issue in qgc vs et)
if (mlPending.wpSendCurrent) { //--DBG TODO AM implement with nav implamntation
memset(&msg, 0, sizeof (mavlink_message_t));
mavlink_msg_mission_current_pack(system_id,
component_id,
&msg, ((uint16_t)mlNavigation.toWP) - 1);
bytes2Send += mavlink_msg_to_send_buffer((UartOutBuff + 1 + bytes2Send), &msg);
mlPending.wpSendCurrent = FALSE;
}
// clear the msg
memset(&msg, 0, sizeof (mavlink_message_t));
//if (mlPending.wpTransaction){ //DBG
CopyMsgToBuff = 1; //Set send msg flag - if nothing to send the switch statement will reset this
switch (mlPending.wpProtState) {
case WP_PROT_LIST_REQUESTED:
//mlPending.statustext++;
//mlStatustext.severity = MAV_SEVERITY_INFO;
//strncpy(mlStatustext.text, "Got mission list request. Sending count...", 49);
mavlink_msg_mission_count_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
mlWpValues.wpCount);
// Change the state machine state
mlPending.wpProtState = WP_PROT_NUM_SENT;
// Reset the timeout
mlPending.wpTimeOut = 0;
bytes2Send += mavlink_msg_to_send_buffer(UartOutBuff, &msg);
break;
case WP_PROT_GETTING_WP_IDLE:
if (mlPending.wpCurrentWpInTransaction < mlPending.wpTotalWps) {
mavlink_msg_mission_request_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
mlPending.wpCurrentWpInTransaction++);
// Change the state machine state
mlPending.wpProtState = WP_PROT_RX_WP;
} else {
mavlink_msg_mission_ack_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
MAV_MISSION_ACCEPTED); // 0 is success
// Update the waypoint count
mlWpValues.wpCount = mlPending.wpTotalWps;
// End the transaction
mlPending.wpTransaction = 0;
mlPending.wpProtState = WP_PROT_IDLE;
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTotalWps = 0;
mlPending.wpSendCurrent = TRUE; // send current waypoint index
// put zeros in the rest of the waypoints;
//clearWaypointsFrom(mlWpValues.wpCount); //this clears the EEPROM WP storage
}
bytes2Send += mavlink_msg_to_send_buffer(UartOutBuff, &msg);
// Reset the timeout
mlPending.wpTimeOut = 0;
break;
case WP_PROT_TX_WP:
//mlPending.statustext++;
//mlStatustext.severity = MAV_SEVERITY_INFO;
//strncpy(mlStatustext.text, "Sending waypoint.", 25);
// Send WP
mavlink_msg_mission_item_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
mlPending.wpCurrentWpInTransaction,
MAV_FRAME_GLOBAL,
mlWpValues.type[mlPending.wpCurrentWpInTransaction],
0, // not current
1, // autocontinue
0.0, // Param 1 not used
0.0, // Param 2 not used
(float) mlWpValues.orbit[mlPending.wpCurrentWpInTransaction],
0.0, // Param 4 not used
mlWpValues.lat[mlPending.wpCurrentWpInTransaction],
mlWpValues.lon[mlPending.wpCurrentWpInTransaction],
mlWpValues.alt[mlPending.wpCurrentWpInTransaction]); // always autocontinue
// Switch the state waiting for the next request
// Change the state machine state
mlPending.wpProtState = WP_PROT_SENDING_WP_IDLE;
bytes2Send += mavlink_msg_to_send_buffer(UartOutBuff, &msg);
// Reset the timeout
mlPending.wpTimeOut = 0;
break;
default:
CopyMsgToBuff = 0;
break;
} // switch wpProtState
// if (CopyMsgToBuff) // Copy the message to the send buffer
// bytes2Send += mavlink_msg_to_send_buffer((UartOutBuff + 1 + bytes2Send), &msg);
//}//if DBG
mlPending.wpTimeOut++;
// if Timed out reset the state machine and send an error
if (mlPending.wpTimeOut > PROTOCOL_TIMEOUT_TICKS) {
memset(&msg, 0, sizeof (mavlink_message_t));
mavlink_msg_mission_ack_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
1); // 1 is failure
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((UartOutBuff + 1 + bytes2Send), &msg);
// reset the state machine
mlPending.wpTransaction = 0;
mlPending.wpProtState = WP_PROT_IDLE;
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
mlPending.wpTotalWps = 0;
}
return(bytes2Send);
}
