forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
system.cpp
269 lines (218 loc) · 7.68 KB
/
system.cpp
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
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
#include "Sub.h"
/*****************************************************************************
* The init_ardupilot function processes everything we need for an in - air restart
* We will determine later if we are actually on the ground and process a
* ground start in that case.
*
*****************************************************************************/
static void failsafe_check_static()
{
sub.mainloop_failsafe_check();
}
void Sub::init_ardupilot()
{
// initialise notify system
notify.init();
// initialise battery monitor
battery.init();
barometer.init();
#if AP_FEATURE_BOARD_DETECT
// Detection won't work until after BoardConfig.init()
switch (AP_BoardConfig::get_board_type()) {
case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
AP_Param::set_default_by_name("BARO_EXT_BUS", 0);
break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK:
AP_Param::set_by_name("BARO_EXT_BUS", 1);
break;
default:
AP_Param::set_default_by_name("BARO_EXT_BUS", 1);
break;
}
#elif CONFIG_HAL_BOARD != HAL_BOARD_LINUX
AP_Param::set_default_by_name("BARO_EXT_BUS", 1);
#endif
#if AP_TEMPERATURE_SENSOR_ENABLED
// In order to preserve Sub's previous AP_TemperatureSensor Behavior we set the Default I2C Bus Here
AP_Param::set_default_by_name("TEMP1_BUS", barometer.external_bus());
#endif
// setup telem slots with serial ports
gcs().setup_uarts();
// initialise rc channels including setting mode
rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM);
rc().init();
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up motors and output to escs
init_joystick(); // joystick initialization
#if AP_RELAY_ENABLED
relay.init();
#endif
/*
* setup the 'main loop is dead' check. Note that this relies on
* the RC library being initialised.
*/
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
// Do GPS init
gps.set_log_gps_bit(MASK_LOG_GPS);
gps.init();
AP::compass().set_log_bit(MASK_LOG_COMPASS);
AP::compass().init();
#if AP_AIRSPEED_ENABLED
airspeed.set_log_bit(MASK_LOG_IMU);
#endif
#if AP_OPTICALFLOW_ENABLED
// initialise optical flow sensor
optflow.init(MASK_LOG_OPTFLOW);
#endif
#if HAL_MOUNT_ENABLED
// initialise camera mount
camera_mount.init();
// This step is necessary so that the servo is properly initialized
camera_mount.set_angle_target(0, 0, 0, false);
// for some reason the call to set_angle_targets changes the mode to mavlink targeting!
camera_mount.set_mode(MAV_MOUNT_MODE_RC_TARGETING);
#endif
#if AP_CAMERA_ENABLED
// initialise camera
camera.init();
#endif
#ifdef USERHOOK_INIT
USERHOOK_INIT
#endif
// Init baro and determine if we have external (depth) pressure sensor
barometer.set_log_baro_bit(MASK_LOG_IMU);
barometer.calibrate(false);
barometer.update();
for (uint8_t i = 0; i < barometer.num_instances(); i++) {
if (barometer.get_type(i) == AP_Baro::BARO_TYPE_WATER) {
barometer.set_primary_baro(i);
depth_sensor_idx = i;
ap.depth_sensor_present = true;
sensor_health.depth = barometer.healthy(depth_sensor_idx); // initialize health flag
break; // Go with the first one we find
}
}
if (!ap.depth_sensor_present) {
// We only have onboard baro
// No external underwater depth sensor detected
barometer.set_primary_baro(0);
ahrs.set_alt_measurement_noise(10.0f); // Readings won't correspond with rest of INS
} else {
ahrs.set_alt_measurement_noise(0.1f);
}
leak_detector.init();
last_pilot_heading = ahrs.yaw_sensor;
// initialise rangefinder
#if AP_RANGEFINDER_ENABLED
init_rangefinder();
#endif
// initialise AP_RPM library
#if AP_RPM_ENABLED
rpm_sensor.init();
#endif
// initialise mission library
mission.init();
// initialise AP_Logger library
#if HAL_LOGGING_ENABLED
logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&sub, &Sub::Log_Write_Vehicle_Startup_Messages, void));
#endif
startup_INS_ground();
// enable CPU failsafe
mainloop_failsafe_enable();
ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
// flag that initialisation has completed
ap.initialised = true;
}
//******************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//******************************************************************************
void Sub::startup_INS_ground()
{
// initialise ahrs (may push imu calibration into the mpu6000 if using that device).
ahrs.init();
ahrs.set_vehicle_class(AP_AHRS::VehicleClass::SUBMARINE);
ahrs.set_fly_forward(false);
// Warm up and calibrate gyro offsets
ins.init(scheduler.get_loop_rate_hz());
// reset ahrs including gyro bias
ahrs.reset();
}
// calibrate gyros - returns true if successfully calibrated
// position_ok - returns true if the horizontal absolute position is ok and home position is set
bool Sub::position_ok()
{
// return false if ekf failsafe has triggered
if (failsafe.ekf) {
return false;
}
// check ekf position estimate
return (ekf_position_ok() || optflow_position_ok());
}
// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set
bool Sub::ekf_position_ok()
{
if (!ahrs.have_inertial_nav()) {
// do not allow navigation with dcm position
return false;
}
// with EKF use filter status and ekf check
nav_filter_status filt_status = inertial_nav.get_filter_status();
// if disarmed we accept a predicted horizontal position
if (!motors.armed()) {
return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs));
}
// once armed we require a good absolute position and EKF must not be in const_pos_mode
return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode);
}
// optflow_position_ok - returns true if optical flow based position estimate is ok
bool Sub::optflow_position_ok()
{
// return immediately if EKF not used
if (!ahrs.have_inertial_nav()) {
return false;
}
// return immediately if neither optflow nor visual odometry is enabled
bool enabled = false;
#if AP_OPTICALFLOW_ENABLED
if (optflow.enabled()) {
enabled = true;
}
#endif
#if HAL_VISUALODOM_ENABLED
if (visual_odom.enabled()) {
enabled = true;
}
#endif
if (!enabled) {
return false;
}
// get filter status from EKF
nav_filter_status filt_status = inertial_nav.get_filter_status();
// if disarmed we accept a predicted horizontal relative position
if (!motors.armed()) {
return (filt_status.flags.pred_horiz_pos_rel);
}
return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode);
}
#if HAL_LOGGING_ENABLED
/*
should we log a message type now?
*/
bool Sub::should_log(uint32_t mask)
{
ap.logging_started = logger.logging_started();
return logger.should_log(mask);
}
#endif
#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
#include <AP_Avoidance/AP_Avoidance.h>
#include <AP_ADSB/AP_ADSB.h>
// dummy method to avoid linking AFS
#if AP_ADVANCEDFAILSAFE_ENABLED
bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) { return false; }
AP_AdvancedFailsafe *AP::advancedfailsafe() { return nullptr; }
#endif
#if HAL_ADSB_ENABLED
// dummy method to avoid linking AP_Avoidance
AP_Avoidance *AP::ap_avoidance() { return nullptr; }
#endif