void CFolderNew::OnFocus(LTBOOL bFocus)
{
if (bFocus)
{
BuildMissionList();
SetSelection(g_pGameClientShell->GetPlayerSummary()->GetNextMission());
}
else
{
SetSelection(kNoSelection);
ClearMissionList();
}
CBaseSelectionFolder::OnFocus(bFocus);
}
/**
* This function implements the mission protocol state machine for the MAVLink protocol.
* events can be passed as the first argument, or NO_EVENT if desired. data is a pointer
* to data if there is any to be passed to the state logic. data is not guaranteed to persist
* beyond the single call to this function.
*/
void MavLinkEvaluateMissionState(uint8_t event, void *data)
{
// Internal counter variable for use with the COUNTDOWN state
static uint16_t counter = 0;
// Keep track of the expected length of the incoming mission list
static uint16_t mavlinkNewMissionListSize;
// Track a mission index for some multi-state loops.
static uint8_t currentMissionIndex;
// Track the state
static uint8_t state = MISSION_STATE_INACTIVE;
// Keep track of the next state to transition into
uint8_t nextState = state;
// Then check the mission protocol state
switch (state) {
case MISSION_STATE_INACTIVE:
if (event == MISSION_EVENT_REQUEST_LIST_RECEIVED) {
AddTransientMessage(MAVLINK_MSG_ID_MISSION_COUNT);
currentMissionIndex = 0;
nextState = MISSION_STATE_SEND_MISSION_COUNT;
} else if (event == MISSION_EVENT_COUNT_RECEIVED) {
// Don't allow for writing of new missions if we're in autonomous mode.
if ((systemStatus.status & 0x0001) > 0) {
AddTransientMessage(MAVLINK_MSG_ID_MISSION_ACK);
nextState = MISSION_STATE_ACK_ERROR;
} else {
uint8_t newListSize = *(uint8_t *)data;
// Only respond with a request if there are missions to request.
// If we received a 0-length mission list, just respond with a MISSION_ACK error
if (newListSize == 0) {
AddTransientMessage(MAVLINK_MSG_ID_MISSION_ACK);
nextState = MISSION_STATE_ACK_ERROR;
} else if (newListSize > mList.maxSize) {
AddTransientMessage(MAVLINK_MSG_ID_MISSION_ACK);
nextState = MISSION_STATE_ACK_NO_SPACE;
}
// Otherwise we're set to start retrieving a new mission list so we request the first mission.
else {
mavlinkNewMissionListSize = newListSize;
ClearMissionList();
currentMissionIndex = 0;
AddTransientMessage(MAVLINK_MSG_ID_MISSION_REQUEST);
nextState = MISSION_STATE_SEND_REQUEST;
}
}
} else if (event == MISSION_EVENT_CLEAR_ALL_RECEIVED) {
// If we're in autonomous mode, don't allow for clearing the mission list
if ((systemStatus.status & 0x0001) > 0) {
AddTransientMessage(MAVLINK_MSG_ID_MISSION_ACK);
nextState = MISSION_STATE_ACK_ERROR;
}
// But if we're in manual mode, go ahead and clear everything.
else {
ClearMissionList();
AddTransientMessage(MAVLINK_MSG_ID_MISSION_ACK);
nextState = MISSION_STATE_ACK_ACCEPTED;
}
} else if (event == MISSION_EVENT_SET_CURRENT_RECEIVED) {
SetCurrentMission(*(uint8_t*)data);
AddTransientMessage(MAVLINK_MSG_ID_MISSION_CURRENT);
nextState = MISSION_STATE_SEND_CURRENT;
}
// If a MISSION_CURRENT message was scheduled and we aren't in any state, just transmit
// it. This is likely due to a waypoint being reached.
else if (event == MISSION_EVENT_CURRENT_DISPATCHED) {
MavLinkSendCurrentMission();
}
break;
case MISSION_STATE_SEND_MISSION_COUNT:
if (event == MISSION_EVENT_COUNT_DISPATCHED) {
MavLinkSendMissionCount();
nextState = MISSION_STATE_COUNTDOWN;
}
break;
case MISSION_STATE_COUNTDOWN:
if (event == MISSION_EVENT_ENTER_STATE) {
counter = 0;
} else if (event == MISSION_EVENT_NONE) {
if (counter++ > 400) {
nextState = MISSION_STATE_INACTIVE;
}
} else if (event == MISSION_EVENT_REQUEST_RECEIVED) {
if (*(uint8_t *)data == currentMissionIndex) {
AddTransientMessage(MAVLINK_MSG_ID_MISSION_ITEM);
nextState = MISSION_STATE_SEND_MISSION_ITEM;
}
} else if (event == MISSION_EVENT_ACK_RECEIVED) {
nextState = MISSION_STATE_INACTIVE;
} else if (event == MISSION_EVENT_ITEM_RECEIVED) {
mavlink_mission_item_t incomingMission = *(mavlink_mission_item_t *)data;
// Make sure that they're coming in in the right order, and if they don't return an error in
// the acknowledgment response.
if (currentMissionIndex == incomingMission.seq) {
Mission m = {
{
incomingMission.x,
incomingMission.y,
incomingMission.z
},
incomingMission.frame,
incomingMission.command,
{
incomingMission.param1,
incomingMission.param2,
incomingMission.param3,
incomingMission.param4
},
incomingMission.autocontinue
};
// Attempt to record this mission to the list, recording the result, which will be 0 for failure.
int8_t missionAddStatus;
AppendMission(&m, &missionAddStatus);
if (missionAddStatus != -1) {
// If this is going to be the new current mission, then we should set it as such.
if (incomingMission.current) {
SetCurrentMission(incomingMission.seq);
}
// If this was the last mission we were expecting, respond with an ACK confirming that we've successfully
// received the entire mission list. Otherwise we just increment and request the next mission.
if (currentMissionIndex == mavlinkNewMissionListSize - 1) {
AddTransientMessage(MAVLINK_MSG_ID_MISSION_ACK);
nextState = MISSION_STATE_ACK_ACCEPTED;
} else {
++currentMissionIndex;
AddTransientMessage(MAVLINK_MSG_ID_MISSION_REQUEST);
nextState = MISSION_STATE_SEND_REQUEST;
}
}
// If we've run out of space before the last message, respond saying so.
else {
AddTransientMessage(MAVLINK_MSG_ID_MISSION_ACK);
nextState = MISSION_STATE_ACK_NO_SPACE;
}
}
}
break;
case MISSION_STATE_SEND_MISSION_ITEM:
if (event == MISSION_EVENT_ITEM_DISPATCHED) {
MavLinkSendMissionItem(currentMissionIndex);
++currentMissionIndex;
nextState = MISSION_STATE_COUNTDOWN;
}
break;
case MISSION_STATE_SEND_CURRENT:
if (event == MISSION_EVENT_CURRENT_DISPATCHED) {
MavLinkSendCurrentMission();
nextState = MISSION_STATE_INACTIVE;
}
break;
case MISSION_STATE_ACK_NO_SPACE:
if (event == MISSION_EVENT_ACK_DISPATCHED) {
MavLinkSendMissionAck(MAV_MISSION_NO_SPACE);
nextState = MISSION_STATE_INACTIVE;
}
break;
case MISSION_STATE_ACK_ERROR:
if (event == MISSION_EVENT_ACK_DISPATCHED) {
MavLinkSendMissionAck(MAV_MISSION_ERROR);
nextState = MISSION_STATE_INACTIVE;
}
break;
case MISSION_STATE_ACK_ACCEPTED:
if (event == MISSION_EVENT_ACK_DISPATCHED) {
MavLinkSendMissionAck(MAV_MISSION_ACCEPTED);
nextState = MISSION_STATE_INACTIVE;
}
break;
case MISSION_STATE_ACK_INVALID_SEQUENCE:
if (event == MISSION_EVENT_ACK_DISPATCHED) {
MavLinkSendMissionAck(MAV_MISSION_INVALID_SEQUENCE);
nextState = MISSION_STATE_INACTIVE;
}
break;
case MISSION_STATE_SEND_REQUEST: {
if (event == MISSION_EVENT_REQUEST_DISPATCHED) {
MavLinkSendMissionRequest(currentMissionIndex);
nextState = MISSION_STATE_COUNTDOWN;
}
} break;
}
// Here is when we actually transition between states, calling init/exit code as necessary
if (nextState != state) {
MavLinkEvaluateMissionState(MISSION_EVENT_EXIT_STATE, NULL);
state = nextState;
MavLinkEvaluateMissionState(MISSION_EVENT_ENTER_STATE, NULL);
}
}
void CFolderNew::Term()
{
ClearMissionList();
CBaseSelectionFolder::Term();
}
