You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
88 lines
2.1 KiB
88 lines
2.1 KiB
|
|
#ifndef __FOLLOWME_STATE_H__ |
|
#define __FOLLOWME_STATE_H__ |
|
|
|
#include <GCS_MAVLink.h> |
|
#include <AP_GPS.h> |
|
|
|
#include "arducopter_defines.h" |
|
|
|
class FMStateMachine { |
|
public: |
|
FMStateMachine() : |
|
_last_run_millis(0), |
|
_loop_period(500), |
|
_last_vehicle_hb_millis(0), |
|
_vehicle_mode(MODE_NUM_MODES), |
|
_vehicle_armed(false), |
|
_vehicle_gps_fix(0), |
|
_vehicle_lat(0), |
|
_vehicle_lon(0), |
|
_vehicle_altitude(0), |
|
/* Don't exactly know what these defaults for target system |
|
* and target component mean - they're derived from mavproxy */ |
|
_target_system(1), |
|
_target_component(1) |
|
{} |
|
|
|
void on_downstream_heartbeat(mavlink_heartbeat_t *pkt); |
|
void on_downstream_gps_raw_int(mavlink_gps_raw_int_t* pkt); |
|
|
|
void on_upstream_command_long(mavlink_command_long_t *pkt); |
|
void on_upstream_set_mode(mavlink_set_mode_t* pkt); |
|
|
|
void on_loop(GPS* gps); |
|
|
|
void on_button_activate(); |
|
void on_button_cancel(); |
|
|
|
private: |
|
/* _send_guide: Send a guide waypoint packet upstream. */ |
|
void _send_guide(); |
|
|
|
/* _send_loiter: Send a setmode loiter packet upstream. */ |
|
void _send_loiter(); |
|
|
|
/* Calculate whether we have sufficient conditions to enter guide mode. */ |
|
bool _check_guide_valid(); |
|
|
|
/* _update_local_gps: Get device's current GPS status and location. Called |
|
* periodically. Can activate _on_fault_cancel(); */ |
|
void _update_local_gps(GPS* gps); |
|
|
|
void _on_user_override(); |
|
void _on_fault_cancel(); |
|
|
|
void _set_guide_offset(); |
|
|
|
/* Set once a start guide mode packet has been sent. |
|
* Unset whenever we stop guiding. */ |
|
bool _guiding; |
|
|
|
/* Scheduling the on_loop periodic updater. */ |
|
uint32_t _last_run_millis; |
|
uint32_t _loop_period; |
|
uint32_t _last_vehicle_hb_millis; |
|
|
|
int8_t _vehicle_mode; |
|
bool _vehicle_armed; |
|
uint8_t _vehicle_gps_fix; |
|
int32_t _vehicle_lat; |
|
int32_t _vehicle_lon; |
|
int32_t _vehicle_altitude; |
|
|
|
uint8_t _target_system; |
|
uint8_t _target_component; |
|
|
|
bool _local_gps_valid; |
|
int32_t _local_gps_lat; |
|
int32_t _local_gps_lon; |
|
int32_t _local_gps_altitude; |
|
|
|
int32_t _offs_lat; |
|
int32_t _offs_lon; |
|
int32_t _offs_altitude; |
|
}; |
|
|
|
#endif // __FOLLOWME_STATE_H__ |
|
|
|
|