/********************************************************************** * 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()