/**********************************************************************
* Function: checkEvents
* @return None
* @remark Checks for various events that can occur, and sets the
* appropriate flags for handling later.
**********************************************************************/
static void checkEvents() {
// Clear all event flags
int i;
for (i=0; i < EVENT_BYTE_SIZE; i++)
event.bytes[i] = 0x0;
// Navigation events
if (Navigation_isDone())
event.flags.navigationDone = TRUE;
if (Navigation_hasError())
setError(Navigation_getError());
// Override
if (Override_isTriggered())
event.flags.receiverDetected = TRUE;
// XBee messages (from command center)
if (Mavlink_hasNewMessage()) {
lastMavlinkMessageID = Mavlink_getNewMessageID();
lastMavlinkCommandID = MAVLINK_NO_COMMAND;
lastMavlinkMessageWantsAck = FALSE;
switch (lastMavlinkMessageID) {
// -------------------------- Acknowledgements ------------------------
case MAVLINK_MSG_ID_MAVLINK_ACK:
// No ACKS from ComPAS to AtLAs
break;
// ------------------------------ Messages ----------------------------
case MAVLINK_MSG_ID_CMD_OTHER:
lastMavlinkMessageWantsAck = Mavlink_newMessage.commandOtherData.ack;
if (Mavlink_newMessage.commandOtherData.command == MAVLINK_RESET_BOAT ) {
event.flags.haveResetMessage = TRUE;
lastMavlinkCommandID = MAVLINK_RESET_BOAT ;
}
else if (Mavlink_newMessage.commandOtherData.command == MAVLINK_RETURN_STATION) {
event.flags.haveReturnStationMessage = TRUE;
overrideShutdown = FALSE;
lastMavlinkCommandID = MAVLINK_RETURN_STATION;
}
else if (Mavlink_newMessage.commandOtherData.command == MAVLINK_SAVE_STATION) {
event.flags.haveSetStationMessage = TRUE;
wantSaveStation = TRUE;
lastMavlinkCommandID = MAVLINK_SAVE_STATION;
}
else if (Mavlink_newMessage.commandOtherData.command == MAVLINK_GEOCENTRIC_ORIGIN) {
event.flags.haveSetOriginMessage = TRUE;
lastMavlinkCommandID = MAVLINK_GEOCENTRIC_ORIGIN;
}
else if (Mavlink_newMessage.commandOtherData.command == MAVLINK_OVERRIDE) {
event.flags.haveOverrideMessage = TRUE;
overrideShutdown = TRUE;
lastMavlinkCommandID = MAVLINK_OVERRIDE;
}
break;
case MAVLINK_MSG_ID_GPS_ECEF:
lastMavlinkMessageWantsAck = Mavlink_newMessage.gpsGeocentricData.ack;
if (Mavlink_newMessage.gpsGeocentricData.status == MAVLINK_GEOCENTRIC_ORIGIN) {
event.flags.haveSetOriginMessage = TRUE;
lastMavlinkCommandID = MAVLINK_GEOCENTRIC_ORIGIN;
}
else if (Mavlink_newMessage.gpsGeocentricData.status == MAVLINK_GEOCENTRIC_ERROR)
event.flags.haveGeocentricErrorMessage = TRUE;
break;
case MAVLINK_MSG_ID_GPS_NED:
lastMavlinkMessageWantsAck = Mavlink_newMessage.gpsLocalData.ack;
if (Mavlink_newMessage.gpsLocalData.status == MAVLINK_LOCAL_SET_STATION) {
event.flags.haveSetStationMessage = TRUE;
lastMavlinkCommandID = MAVLINK_LOCAL_SET_STATION;
wantSaveStation = FALSE;
}
else if (Mavlink_newMessage.gpsLocalData.status == MAVLINK_LOCAL_START_RESCUE) {
event.flags.haveStartRescueMessage = TRUE;
lastMavlinkCommandID = MAVLINK_LOCAL_START_RESCUE;
}
break;
default:
// Unknown message ID
event.flags.haveUnknownMessage = TRUE;
DBPRINT("Mavlink received unhandled message: 0x%X\n");
break;
}
}
/* If the receiver is off, boat is not stopped, and we either
have a station or are setting one, then turn override off */
if (!event.flags.receiverDetected && !overrideShutdown
&& (haveStation || (state == STATE_SETSTATION))) {
event.flags.wantOverride = FALSE;
}
else {
event.flags.wantOverride = TRUE;
}
} // checkEvents()
static void doWatchdog(void) {
// Clear all event flags
int i;
for (i=0; i < EVENT_BYTE_SIZE; i++)
event.bytes[i] = 0x0;
if (Mavlink_hasNewMessage()) {
lastMavlinkMessageID = Mavlink_getNewMessageID();
lastMavlinkCommandID = MAVLINK_NO_COMMAND;
lastMavlinkMessageWantsAck = FALSE;
if (Mavlink_hasHeartbeat()) {
DBPRINT("A: heartbeat!\n");
}
switch (lastMavlinkMessageID) {
/*-------------------- Acknowledgement messages ------------------ */
case MAVLINK_MSG_ID_MAVLINK_ACK:
// ---- Messages from AtLAs to ComPAS (from Compas.c) --------
// CMD_OTHER
if (Mavlink_newMessage.ackData.msgID == MAVLINK_MSG_ID_CMD_OTHER) {
// Responding to a return to station request
if (Mavlink_newMessage.ackData.msgStatus == MAVLINK_RETURN_STATION) {
event.flags.haveReturnStationAck = TRUE;
DBPRINT("A: ack %s\n", getMessage(RETURNING_MESSAGE));
}
// Boat is in override state
else if (Mavlink_newMessage.ackData.msgStatus == MAVLINK_OVERRIDE) {
event.flags.haveStopAck = TRUE;
DBPRINT("A: ack %s\n", getMessage(STOPPING_BOAT_MESSAGE));
}
// Boat saved station
else if (Mavlink_newMessage.ackData.msgStatus == MAVLINK_SAVE_STATION) {
event.flags.haveSetStationAck = TRUE;
DBPRINT("A: ack %s\n", getMessage(SAVING_STATION_MESSAGE));
}
}
// GPS_NED
else if (Mavlink_newMessage.ackData.msgID == MAVLINK_MSG_ID_GPS_NED) {
// Responding to a rescue
if (Mavlink_newMessage.ackData.msgStatus == MAVLINK_LOCAL_START_RESCUE) {
event.flags.haveStartRescueAck = TRUE;
DBPRINT("A: ack %s\n", getMessage(STARTING_RESCUE_MESSAGE));
}
else if (Mavlink_newMessage.ackData.msgStatus == MAVLINK_LOCAL_SET_STATION) {
event.flags.haveSetStationAck = TRUE;
DBPRINT("A: ack %s\n", getMessage(SET_STATION_MESSAGE));
}
}
// GPS_ECEF
else if (Mavlink_newMessage.ackData.msgID == MAVLINK_MSG_ID_GPS_ECEF) {
if (Mavlink_newMessage.ackData.msgStatus == MAVLINK_GEOCENTRIC_ORIGIN) {
event.flags.haveSetOriginAck = TRUE;
DBPRINT("A: ack=%s\n", getMessage(SET_ORIGIN_MESSAGE));
}
}
// --- Messages from ComPAS to AtLAs (from Atlas.c) ---
// No ACKS from ComPAS to AtLAs
break;
/*------------------ CMD_OTHER messages --------------- */
case MAVLINK_MSG_ID_CMD_OTHER:
{
// --- Messages from ComPAS to AtLAs (from Atlas.c) ---
lastMavlinkMessageWantsAck = Mavlink_newMessage.commandOtherData.ack;
char *wantAck = "";
if (Mavlink_newMessage.commandOtherData.ack == WANT_ACK)
wantAck = " [WANT_ACK]";
if (Mavlink_newMessage.commandOtherData.command == MAVLINK_RESET_BOAT ) {
event.flags.haveResetMessage = TRUE;
DBPRINT("C: %s%s\n",getMessage(RESET_BOAT_MESSAGE),wantAck);
}
else if (Mavlink_newMessage.commandOtherData.command == MAVLINK_RETURN_STATION) {
event.flags.haveReturnStationMessage = TRUE;
DBPRINT("C: %s%s\n",getMessage(START_RETURN_MESSAGE),wantAck);
}
else if (Mavlink_newMessage.commandOtherData.command == MAVLINK_SAVE_STATION) {
event.flags.haveSetStationMessage = TRUE;
DBPRINT("C: %s%s\n",getMessage(SET_STATION_MESSAGE),wantAck);
}
else if (Mavlink_newMessage.commandOtherData.command == MAVLINK_OVERRIDE) {
event.flags.haveOverrideMessage = TRUE;
DBPRINT("C: %s%s\n",getMessage(STOPPING_BOAT_MESSAGE),wantAck);
}
// ---- Messages from AtLAs to ComPAS (from Compas.c) --------
else if (Mavlink_newMessage.commandOtherData.command == MAVLINK_REQUEST_ORIGIN) {
event.flags.haveRequestOriginMessage = TRUE;
DBPRINT("A: requesting origin.\n");
}
break;
}
/*-------------------- GPS messages ------------------ */
case MAVLINK_MSG_ID_GPS_ECEF:
// --- Messages from ComPAS to AtLAs (from Atlas.c) ---
lastMavlinkMessageWantsAck = Mavlink_newMessage.gpsGeocentricData.ack;
if (Mavlink_newMessage.gpsGeocentricData.status == MAVLINK_GEOCENTRIC_ORIGIN) {
event.flags.haveSetOriginMessage = TRUE;
DBPRINT("C: Sending boat origin.\n");
DBPRINT(" X=%.0f, Y=%.0f, Z=%.0f\n", Mavlink_newMessage.gpsGeocentricData.x,
Mavlink_newMessage.gpsGeocentricData.y, Mavlink_newMessage.gpsGeocentricData.z );
}
else if (Mavlink_newMessage.gpsGeocentricData.status == MAVLINK_GEOCENTRIC_ERROR) {
event.flags.haveGeocentricErrorMessage = TRUE;
DBPRINT("C: gps err X=%.0f, Y=%.0f, Z=%.0f\n", Mavlink_newMessage.gpsGeocentricData.x,
Mavlink_newMessage.gpsGeocentricData.y, Mavlink_newMessage.gpsGeocentricData.z );
}
// ---- Messages from AtLAs to ComPAS (from Compas.c) --------
break;
case MAVLINK_MSG_ID_GPS_NED:
// --- Messages from ComPAS to AtLAs (from Atlas.c) ---
lastMavlinkMessageWantsAck = Mavlink_newMessage.gpsLocalData.ack;
if (Mavlink_newMessage.gpsLocalData.status == MAVLINK_LOCAL_SET_STATION) {
event.flags.haveSetStationMessage = TRUE;
lastMavlinkCommandID = MAVLINK_LOCAL_SET_STATION;
DBPRINT("C: %s\n", getMessage(SET_STATION_MESSAGE));
DBPRINT(" N=%.1f, E=%.1f\n", Mavlink_newMessage.gpsLocalData.north,
Mavlink_newMessage.gpsLocalData.east);
}
else if (Mavlink_newMessage.gpsLocalData.status == MAVLINK_LOCAL_START_RESCUE) {
event.flags.haveStartRescueMessage = TRUE;
lastMavlinkCommandID = MAVLINK_LOCAL_START_RESCUE;
DBPRINT("C: %s\n", getMessage(STARTING_RESCUE_MESSAGE));
DBPRINT(" N=%.1f, E=%.1f\n", Mavlink_newMessage.gpsLocalData.north,
Mavlink_newMessage.gpsLocalData.east);
}
// ---- Messages from AtLAs to ComPAS (from Compas.c) -------- // Boat sent position info
else if (Mavlink_newMessage.gpsLocalData.status == MAVLINK_LOCAL_BOAT_POSITION) {
event.flags.haveBoatPositionMessage = TRUE;
DBPRINT("A: pos N=%.1f, E=$.1f\n", Mavlink_newMessage.gpsLocalData.north,
Mavlink_newMessage.gpsLocalData.east);
}
break;
/*---------------- Status and Error messages ---------- */
case MAVLINK_MSG_ID_STATUS_AND_ERROR:
// ---------- Boat error messages ------------------
if (Mavlink_newMessage.statusAndErrorData.error != ERROR_NONE) {
lastBoatErrorCode = Mavlink_newMessage.statusAndErrorData.error;
DBPRINT("A: err=%s\n", getErrorMessage(lastBoatErrorCode));
}
else {
// ---------- Boat status messages -----------------
if (Mavlink_newMessage.statusAndErrorData.status == MAVLINK_STATUS_ONLINE) {
event.flags.haveBoatOnlineMessage = TRUE;
DBPRINT("A: %s\n", getMessage(BOAT_ONLINE_MESSAGE));
}
else if (Mavlink_newMessage.statusAndErrorData.status == MAVLINK_STATUS_START_RESCUE) {
DBPRINT("A: started a rescue\n");
}
else if (Mavlink_newMessage.statusAndErrorData.status == MAVLINK_STATUS_RESCUE_SUCCESS) {
DBPRINT("A: successfully rescued person\n");
}
else if (Mavlink_newMessage.statusAndErrorData.status == MAVLINK_STATUS_RETURN_STATION) {
DBPRINT("A: returning to station\n");
}
else if (Mavlink_newMessage.statusAndErrorData.status == MAVLINK_STATUS_ARRIVED_STATION) {
DBPRINT("A: arrived at station\n");
}
else if (Mavlink_newMessage.statusAndErrorData.status == MAVLINK_STATUS_OVERRIDE) {
DBPRINT("A: in override mode\n");
}
else {
DBPRINT("Unknown status: 0x%X\n",
Mavlink_newMessage.statusAndErrorData.status);
}
}
break;
/*---------------- Barometer altitude message ----------------*/
case MAVLINK_MSG_ID_DATA:
event.flags.haveBarometerMessage = TRUE;
DBPRINT("A: data A=%.2f, T=%.2f,\n lipo=%.2f, nimh=%.2f\n",
Mavlink_newMessage.telemetryData.altitude,
Mavlink_newMessage.telemetryData.temperature,
(float)Mavlink_newMessage.telemetryData.batVolt1/1000,
(float)Mavlink_newMessage.telemetryData.batVolt2/1000);
break;
case MAVLINK_MSG_ID_DEBUG:
{
char *sender = "";
if (Mavlink_newMessage.debugData.sender == MAVLINK_SENDER_ATLAS)
sender = "A";
else if (Mavlink_newMessage.debugData.sender == MAVLINK_SENDER_COMPAS)
sender = "C";
else
sender = "?";
DBPRINT("%s: debug=%s\n", sender, Mavlink_newMessage.debugData.message);
break;
}
default:
// Unknown message ID
event.flags.haveUnknownMessage = TRUE;
DBPRINT("Unknown: 0x%X\n", lastMavlinkMessageID);
break;
} // switch
}
} // doWatchdog()
