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
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
|
#ifndef AIRDOG_H
#define AIRDOG_H
#include <nuttx/config.h>
#include <nuttx/sched.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <fcntl.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/airdog_status.h>
#include <uORB/topics/airdog_path_log.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_rgbled.h>
#include <drivers/drv_tone_alarm.h>
#include <drivers/drv_hrt.h>
#include <commander/px4_custom_mode.h>
#include <commander/commander_helper.h>
#include <board_config.h>
#include <mavlink/mavlink_log.h>
#include "paramhandler.h"
#include "button_controller.h"
#include "i2c_controller.h"
#include "i2c_display_controller.h"
#include "menu_controller.h"
#include "common.h"
#define LISTENER_ADDR 0x20 /**< I2C adress of our button i2c controller */
#define DISPLAY_ADDR 0x38 /**< I2C adress of our display i2c controller */
class cAirdog
{
public:
cAirdog();
~cAirdog();
void start();
void cycle();
struct work_s work;
bool button_pressed_i2c(uint8_t button, hrt_abstime time);
bool button_clicked_i2c(uint8_t button, bool long_press);
private:
enum MAV_MODE_FLAG {
MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
MAV_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
MAV_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
MAV_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
MAV_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */
};
void send_set_mode(uint8_t base_mode, enum PX4_CUSTOM_MAIN_MODE custom_main_mode);
// void send_set_state(enum NAV_STATE state, enum AUTO_MOVE_DIRECTION direction);
// void send_set_move(enum AUTO_MOVE_DIRECTION direction);
void send_save_params();
void send_set_parameter_cmd(char *param_name, int int_val, bool should_save);
void send_get_parameter_value_cmd(char *param_name);
void send_record_path_cmd(bool start);
void set_land_mode();
void display_drone_state();
void load_param(int menu_param);
void set_param_if_updated();
void handle_takeoff();
void display_discharged_mah();
bool running;
uint8_t base_mode;
int airdog_status_sub;
int vehicle_status_sub;
orb_advert_t cmd_pub;
orb_advert_t cmd_log_start;
int buzzer;
I2C_CONTROLLER *pi2c_ctrl;
I2C_DISPLAY_CONTROLLER *pi2c_disp_ctrl;
MENU_CONTROLLER *pmenu_ctrl;
cParamHandler *pparam_handler;
cButtonController *pbutton_ctrl;
int mavlink_fd;
bool hil;
bool armed;
bool drone_active;
bool log_running;
struct airdog_status_s airdog_status;
struct vehicle_status_s vehicle_status;
uint64_t last_drone_timestamp;
uint64_t takeoff_request_time;
bool takeoff_requested;
param_t param_bat_warn;
param_t param_bat_failsafe;
param_t param_trainer_id;
float bat_warning_level;
float bat_critical_level;
bool rtl_triggered_from_battery;
int32_t trainer_remote_id;
bool pitch_down;
int battery_warning_count;
};
#endif // AIRDOG_H
|