/
MissionMonitor.h
81 lines (63 loc) · 2.31 KB
/
MissionMonitor.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
// MissionMonitor.h
#ifndef _MISSIONMONITOR_h
#define _MISSIONMONITOR_h
#if defined(ARDUINO) && ARDUINO >= 100
#include "arduino.h"
#else
#include "WProgram.h"
#endif
#include <mavlink_2_ardupilot.h>
#include "MAVLinkEventReceiver.h"
#include "ServoRelay.h"
#include "AudioPlayer.h"
/**
* @brief Mission monitor receives events from the MAVLink reader and evaluates the condition of a mission. Its purpose is to shutdown the rover if it is off course.
*/
class MissionMonitor : public MAVLinkEventReceiver
{
public:
MissionMonitor( uint32_t secondsBeforeEmergencyStop, GPS_FIX_TYPE lowestGpsFixTpye, AudioPlayer* audioPlayer );
virtual void onHeatbeat( mavlink_heartbeat_t mavlink_heartbeat );
virtual void onMissionItemReached( mavlink_mission_item_reached_t mavlink_mission_item_reached );
virtual void onNavControllerOutput( mavlink_nav_controller_output_t mavlink_nav_controller );
virtual void onMissionCurrent( mavlink_mission_current_t mavlink_mission_current );
virtual void onGPSRawInt( mavlink_gps_raw_int_t mavlink_gps_raw_int );
virtual void onGPS2Raw( mavlink_gps2_raw_t mavlink_gps2_raw );
/**
* @brief Used by the scheduling system to give MissionMonitor execution time.
*/
virtual void tick();
protected:
/**
* @brief Used to determine if the mission has gone off course.
*/
virtual void evaluateMission();
/**
* @brief Centrailized logic for stopping the rover during mission failure.
*/
virtual void failMission();
/**
* @brief Centralized logic for starting and restarting rover monitoring
*/
virtual void start();
virtual void play( ROVER_MODE roverMode);
ROVER_MODE _roverMode = ROVER_MODE_INITIALIZING; // Initialize the rover filght mode
MAV_MODE_FLAG _mavModeFlag = MAV_MODE_FLAG_ENUM_END;
int16_t _lastDistanceToWaypoint = -1;
unsigned long _lastProgressMadeTimeMilliseconds = 0;
unsigned long _lastHeartbeatTimeMilliseconds = 0;
uint16_t _currentWaypointSequenceId = 0;
bool _firstHeartbeat = false;
bool _firstTick = false;
bool _isFailed = false;
bool _wrongDirection = false;
uint32_t _wrongDirectionCount = 0;
uint32_t _secondsBeforeEmergencyStop = 20;
GPS_FIX_TYPE _gps1FixType = GPS_FIX_TYPE_NO_GPS;
GPS_FIX_TYPE _gps2FixType = GPS_FIX_TYPE_NO_GPS;
GPS_FIX_TYPE _lowestGpsFixTpye = GPS_FIX_TYPE_NO_GPS;
private:
ServoRelay _servoRelay;
AudioPlayer* _audioPlayer;
};
#endif