CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AP_BoardConfig/AP_BoardConfig.cpp
Views: 1798
1
/*
2
This program is free software: you can redistribute it and/or modify
3
it under the terms of the GNU General Public License as published by
4
the Free Software Foundation, either version 3 of the License, or
5
(at your option) any later version.
6
7
This program is distributed in the hope that it will be useful,
8
but WITHOUT ANY WARRANTY; without even the implied warranty of
9
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10
GNU General Public License for more details.
11
12
You should have received a copy of the GNU General Public License
13
along with this program. If not, see <http://www.gnu.org/licenses/>.
14
*/
15
/*
16
* AP_BoardConfig - board specific configuration
17
*/
18
19
#include "AP_BoardConfig.h"
20
21
#include <AP_Common/AP_Common.h>
22
#include <AP_HAL/AP_HAL.h>
23
#include <AP_Math/AP_Math.h>
24
#include <AP_RTC/AP_RTC.h>
25
#include <AP_Vehicle/AP_Vehicle_Type.h>
26
#include <GCS_MAVLink/GCS.h>
27
#include <AP_Filesystem/AP_Filesystem.h>
28
#include <GCS_MAVLink/GCS.h>
29
#include <GCS_MAVLink/GCS_MAVLink.h>
30
31
#include <stdio.h>
32
33
#ifndef BOARD_TYPE_DEFAULT
34
#define BOARD_TYPE_DEFAULT PX4_BOARD_AUTO
35
#endif
36
37
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
38
#ifndef BOARD_SER1_RTSCTS_DEFAULT
39
# define BOARD_SER1_RTSCTS_DEFAULT 2
40
#endif
41
#ifndef BOARD_TYPE_DEFAULT
42
# define BOARD_TYPE_DEFAULT PX4_BOARD_AUTO
43
#endif
44
#endif
45
46
#ifndef BOARD_SAFETY_ENABLE_DEFAULT
47
#if defined(HAL_GPIO_PIN_SAFETY_IN)
48
// have safety startup enabled if we have a safety pin
49
# define BOARD_SAFETY_ENABLE_DEFAULT 1
50
#elif defined(HAL_WITH_IO_MCU)
51
// if we have an IOMCU then enable by default
52
# define BOARD_SAFETY_ENABLE_DEFAULT HAL_WITH_IO_MCU
53
#else
54
# define BOARD_SAFETY_ENABLE_DEFAULT 0
55
#endif
56
#endif
57
58
#ifndef HAL_IMU_TEMP_DEFAULT
59
#define HAL_IMU_TEMP_DEFAULT -1 // disabled
60
#endif
61
62
#ifndef HAL_IMU_TEMP_MARGIN_LOW_DEFAULT
63
#define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 0 // disabled
64
#endif
65
66
#ifndef BOARD_SAFETY_OPTION_DEFAULT
67
# define BOARD_SAFETY_OPTION_DEFAULT (BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF|BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)
68
#endif
69
#ifndef BOARD_SAFETY_ENABLE
70
# define BOARD_SAFETY_ENABLE 1
71
#endif
72
73
#ifndef BOARD_CONFIG_BOARD_VOLTAGE_MIN
74
#define BOARD_CONFIG_BOARD_VOLTAGE_MIN 4.3f
75
#endif
76
77
#ifndef HAL_BRD_OPTIONS_DEFAULT
78
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) && !APM_BUILD_TYPE(APM_BUILD_Replay)
79
#ifdef HAL_DEBUG_BUILD
80
#define HAL_BRD_OPTIONS_DEFAULT BOARD_OPTION_WATCHDOG | BOARD_OPTION_DEBUG_ENABLE
81
#else
82
#define HAL_BRD_OPTIONS_DEFAULT BOARD_OPTION_WATCHDOG
83
#endif
84
#else
85
#define HAL_BRD_OPTIONS_DEFAULT 0
86
#endif
87
#endif
88
89
#ifndef HAL_DEFAULT_BOOT_DELAY
90
#define HAL_DEFAULT_BOOT_DELAY 0
91
#endif
92
93
extern const AP_HAL::HAL& hal;
94
AP_BoardConfig *AP_BoardConfig::_singleton;
95
96
// constructor
97
AP_BoardConfig::AP_BoardConfig()
98
#if HAL_HAVE_IMU_HEATER
99
// initialise heater PI controller. Note we do this in the cpp file
100
// for ccache efficiency
101
: heater{{HAL_IMUHEAT_P_DEFAULT, HAL_IMUHEAT_I_DEFAULT, 70},}
102
#endif
103
{
104
_singleton = this;
105
AP_Param::setup_object_defaults(this, var_info);
106
};
107
108
// table of user settable parameters
109
const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
110
111
// index 0 was used by PWM_COUNT
112
113
#if AP_FEATURE_RTSCTS
114
#ifdef HAL_HAVE_RTSCTS_SERIAL1
115
// @Param: SER1_RTSCTS
116
// @DisplayName: Serial 1 flow control
117
// @Description: Enable flow control on serial 1 (telemetry 1). You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup. Note that the PX4v1 does not have hardware flow control pins on this port, so you should leave this disabled.
118
// @Values: 0:Disabled,1:Enabled,2:Auto,3:RS-485 Driver enable RTS pin
119
// @RebootRequired: True
120
// @User: Advanced
121
AP_GROUPINFO("SER1_RTSCTS", 1, AP_BoardConfig, state.ser_rtscts[1], BOARD_SER1_RTSCTS_DEFAULT),
122
#endif
123
124
#ifdef HAL_HAVE_RTSCTS_SERIAL2
125
// @Param: SER2_RTSCTS
126
// @CopyFieldsFrom: BRD_SER1_RTSCTS
127
// @DisplayName: Serial 2 flow control
128
// @Description: Enable flow control on serial 2 (telemetry 2). You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup.
129
AP_GROUPINFO("SER2_RTSCTS", 2, AP_BoardConfig, state.ser_rtscts[2], 2),
130
#endif
131
132
#ifdef HAL_HAVE_RTSCTS_SERIAL3
133
// @Param: SER3_RTSCTS
134
// @CopyFieldsFrom: BRD_SER1_RTSCTS
135
// @DisplayName: Serial 3 flow control
136
// @Description: Enable flow control on serial 3. You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup.
137
AP_GROUPINFO("SER3_RTSCTS", 26, AP_BoardConfig, state.ser_rtscts[3], 2),
138
#endif
139
140
#ifdef HAL_HAVE_RTSCTS_SERIAL4
141
// @Param: SER4_RTSCTS
142
// @CopyFieldsFrom: BRD_SER1_RTSCTS
143
// @DisplayName: Serial 4 flow control
144
// @Description: Enable flow control on serial 4. You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup.
145
AP_GROUPINFO("SER4_RTSCTS", 27, AP_BoardConfig, state.ser_rtscts[4], 2),
146
#endif
147
148
#ifdef HAL_HAVE_RTSCTS_SERIAL5
149
// @Param: SER5_RTSCTS
150
// @CopyFieldsFrom: BRD_SER1_RTSCTS
151
// @DisplayName: Serial 5 flow control
152
// @Description: Enable flow control on serial 5. You must have the RTS and CTS pins connected to your radio. The standard DF13 6 pin connector for a 3DR radio does have those pins connected. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup.
153
AP_GROUPINFO("SER5_RTSCTS", 25, AP_BoardConfig, state.ser_rtscts[5], 2),
154
#endif
155
#endif
156
157
// @Param: SAFETY_DEFLT
158
// @DisplayName: Sets default state of the safety switch
159
// @Description: This controls the default state of the safety switch at startup. When set to 1 the safety switch will start in the safe state (flashing) at boot. When set to zero the safety switch will start in the unsafe state (solid) at startup. Note that if a safety switch is fitted the user can still control the safety state after startup using the switch. The safety state can also be controlled in software using a MAVLink message.
160
// @Values: 0:Disabled,1:Enabled
161
// @RebootRequired: True
162
// @User: Standard
163
AP_GROUPINFO("SAFETY_DEFLT", 3, AP_BoardConfig, state.safety_enable, BOARD_SAFETY_ENABLE_DEFAULT),
164
165
#if AP_FEATURE_SBUS_OUT
166
// @Param: SBUS_OUT
167
// @DisplayName: SBUS output rate
168
// @Description: This sets the SBUS output frame rate in Hz
169
// @Values: 0:Disabled,1:50Hz,2:75Hz,3:100Hz,4:150Hz,5:200Hz,6:250Hz,7:300Hz
170
// @RebootRequired: True
171
// @User: Advanced
172
AP_GROUPINFO("SBUS_OUT", 4, AP_BoardConfig, state.sbus_out_rate, 0),
173
#endif
174
175
// @Param: SERIAL_NUM
176
// @DisplayName: User-defined serial number
177
// @Description: User-defined serial number of this vehicle, it can be any arbitrary number you want and has no effect on the autopilot
178
// @Range: -8388608 8388607
179
// @User: Standard
180
AP_GROUPINFO("SERIAL_NUM", 5, AP_BoardConfig, vehicleSerialNumber, 0),
181
182
// @Param: SAFETY_MASK
183
// @DisplayName: Outputs which ignore the safety switch state
184
// @Description: A bitmask which controls what outputs can move while the safety switch has not been pressed
185
// @Bitmask: 0:Output1,1:Output2,2:Output3,3:Output4,4:Output5,5:Output6,6:Output7,7:Output8,8:Output9,9:Output10,10:Output11,11:Output12,12:Output13,13:Output14
186
// @RebootRequired: True
187
// @User: Advanced
188
AP_GROUPINFO("SAFETY_MASK", 7, AP_BoardConfig, state.ignore_safety_channels, 0),
189
190
#if HAL_HAVE_IMU_HEATER
191
// @Param: HEAT_TARG
192
// @DisplayName: Board heater temperature target
193
// @Description: Board heater target temperature for boards with controllable heating units. Set to -1 to disable the heater, please reboot after setting to -1.
194
// @Range: -1 80
195
// @Units: degC
196
// @User: Advanced
197
AP_GROUPINFO("HEAT_TARG", 8, AP_BoardConfig, heater.imu_target_temperature, HAL_IMU_TEMP_DEFAULT),
198
#endif
199
200
#if AP_FEATURE_BOARD_DETECT
201
// @Param: TYPE
202
// @DisplayName: Board type
203
// @Description: This allows selection of a PX4 or VRBRAIN board type. If set to zero then the board type is auto-detected (PX4)
204
// @Values: 0:AUTO,1:PX4V1,2:Pixhawk,3:Cube/Pixhawk2,4:Pixracer,5:PixhawkMini,6:Pixhawk2Slim,13:Intel Aero FC,14:Pixhawk Pro,20:AUAV2.1,21:PCNC1,22:MINDPXV2,23:SP01,24:CUAVv5/FMUV5,30:VRX BRAIN51,32:VRX BRAIN52,33:VRX BRAIN52E,34:VRX UBRAIN51,35:VRX UBRAIN52,36:VRX CORE10,38:VRX BRAIN54,39:PX4 FMUV6,100:PX4 OLDDRIVERS
205
// @RebootRequired: True
206
// @User: Advanced
207
AP_GROUPINFO("TYPE", 9, AP_BoardConfig, state.board_type, BOARD_TYPE_DEFAULT),
208
#endif
209
210
#if HAL_WITH_IO_MCU
211
// @Param: IO_ENABLE
212
// @DisplayName: Enable IO co-processor
213
// @Description: This allows for the IO co-processor on boards with an IOMCU to be disabled. Setting to 2 will enable the IOMCU but not attempt to update firmware on startup
214
// @Values: 0:Disabled,1:Enabled,2:EnableNoFWUpdate
215
// @RebootRequired: True
216
// @User: Advanced
217
AP_GROUPINFO("IO_ENABLE", 10, AP_BoardConfig, state.io_enable, 1),
218
#endif
219
220
#if AP_RADIO_ENABLED
221
// @Group: RADIO
222
// @Path: ../AP_Radio/AP_Radio.cpp
223
AP_SUBGROUPINFO(_radio, "RADIO", 11, AP_BoardConfig, AP_Radio),
224
#endif
225
226
// @Param: SAFETYOPTION
227
// @DisplayName: Options for safety button behavior
228
// @Description: This controls the activation of the safety button. It allows you to control if the safety button can be used for safety enable and/or disable, and whether the button is only active when disarmed
229
// @Bitmask: 0:ActiveForSafetyDisable,1:ActiveForSafetyEnable,2:ActiveWhenArmed,3:Force safety on when the aircraft disarms
230
// @User: Standard
231
AP_GROUPINFO("SAFETYOPTION", 13, AP_BoardConfig, state.safety_option, BOARD_SAFETY_OPTION_DEFAULT),
232
233
#if AP_RTC_ENABLED
234
// @Group: RTC
235
// @Path: ../AP_RTC/AP_RTC.cpp
236
AP_SUBGROUPINFO(rtc, "RTC", 14, AP_BoardConfig, AP_RTC),
237
#endif
238
239
#if HAL_HAVE_BOARD_VOLTAGE
240
// @Param: VBUS_MIN
241
// @DisplayName: Autopilot board voltage requirement
242
// @Description: Minimum voltage on the autopilot power rail to allow the aircraft to arm. 0 to disable the check.
243
// @Units: V
244
// @Range: 4.0 5.5
245
// @Increment: 0.1
246
// @User: Advanced
247
AP_GROUPINFO("VBUS_MIN", 15, AP_BoardConfig, _vbus_min, BOARD_CONFIG_BOARD_VOLTAGE_MIN),
248
249
#endif
250
251
#if HAL_HAVE_SERVO_VOLTAGE
252
// @Param: VSERVO_MIN
253
// @DisplayName: Servo voltage requirement
254
// @Description: Minimum voltage on the servo rail to allow the aircraft to arm. 0 to disable the check.
255
// @Units: V
256
// @Range: 3.3 12.0
257
// @Increment: 0.1
258
// @User: Advanced
259
AP_GROUPINFO("VSERVO_MIN", 16, AP_BoardConfig, _vservo_min, 0),
260
#endif
261
262
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
263
// @Param: SD_SLOWDOWN
264
// @DisplayName: microSD slowdown
265
// @Description: This is a scaling factor to slow down microSD operation. It can be used on flight board and microSD card combinations where full speed is not reliable. For normal full speed operation a value of 0 should be used.
266
// @Range: 0 32
267
// @Increment: 1
268
// @User: Advanced
269
AP_GROUPINFO("SD_SLOWDOWN", 17, AP_BoardConfig, _sdcard_slowdown, 0),
270
#endif
271
272
#ifdef HAL_GPIO_PWM_VOLT_PIN
273
// @Param: PWM_VOLT_SEL
274
// @DisplayName: Set PWM Out Voltage
275
// @Description: This sets the voltage max for PWM output pulses. 0 for 3.3V and 1 for 5V output. On boards with an IOMCU that support this parameter this option only affects the 8 main outputs, not the 6 auxiliary outputs. Using 5V output can help to reduce the impact of ESC noise interference corrupting signals to the ESCs.
276
// @Values: 0:3.3V,1:5V
277
// @User: Advanced
278
AP_GROUPINFO("PWM_VOLT_SEL", 18, AP_BoardConfig, _pwm_volt_sel, 0),
279
#endif
280
281
// @Param: OPTIONS
282
// @DisplayName: Board options
283
// @Description: Board specific option flags
284
// @Bitmask: 0:Enable hardware watchdog, 1:Disable MAVftp, 2:Enable set of internal parameters, 3:Enable Debug Pins, 4:Unlock flash on reboot, 5:Write protect firmware flash on reboot, 6:Write protect bootloader flash on reboot, 7:Skip board validation, 8:Disable board arming gpio output change on arm/disarm, 9:Use safety pins as profiled
285
// @User: Advanced
286
AP_GROUPINFO("OPTIONS", 19, AP_BoardConfig, _options, HAL_BRD_OPTIONS_DEFAULT),
287
288
// @Param: BOOT_DELAY
289
// @DisplayName: Boot delay
290
// @Description: This adds a delay in milliseconds to boot to ensure peripherals initialise fully
291
// @Range: 0 10000
292
// @Units: ms
293
// @User: Advanced
294
AP_GROUPINFO("BOOT_DELAY", 20, AP_BoardConfig, _boot_delay_ms, HAL_DEFAULT_BOOT_DELAY),
295
296
#if HAL_HAVE_IMU_HEATER
297
// @Param: HEAT_P
298
// @DisplayName: Board Heater P gain
299
// @Description: Board Heater P gain
300
// @Range: 1 500
301
// @Increment: 1
302
// @User: Advanced
303
304
// @Param: HEAT_I
305
// @DisplayName: Board Heater I gain
306
// @Description: Board Heater integrator gain
307
// @Range: 0 1
308
// @Increment: 0.1
309
// @User: Advanced
310
311
// @Param: HEAT_IMAX
312
// @DisplayName: Board Heater IMAX
313
// @Description: Board Heater integrator maximum
314
// @Range: 0 100
315
// @Increment: 1
316
// @User: Advanced
317
AP_SUBGROUPINFO(heater.pi_controller, "HEAT_", 21, AP_BoardConfig, AC_PI),
318
#endif
319
320
#ifdef HAL_PIN_ALT_CONFIG
321
// @Param: ALT_CONFIG
322
// @DisplayName: Alternative HW config
323
// @Description: Select an alternative hardware configuration. A value of zero selects the default configuration for this board. Other values are board specific. Please see the documentation for your board for details on any alternative configuration values that may be available.
324
// @Range: 0 10
325
// @Increment: 1
326
// @User: Advanced
327
// @RebootRequired: True
328
AP_GROUPINFO("ALT_CONFIG", 22, AP_BoardConfig, _alt_config, 0),
329
#endif // HAL_PIN_ALT_CONFIG
330
331
#if HAL_HAVE_IMU_HEATER
332
// @Param: HEAT_LOWMGN
333
// @DisplayName: Board heater temp lower margin
334
// @Description: Arming check will fail if temp is lower than this margin below BRD_HEAT_TARG. 0 disables the low temperature check
335
// @Range: 0 20
336
// @Units: degC
337
// @User: Advanced
338
AP_GROUPINFO("HEAT_LOWMGN", 23, AP_BoardConfig, heater.imu_arming_temperature_margin_low, HAL_IMU_TEMP_MARGIN_LOW_DEFAULT),
339
#endif
340
341
#if AP_SDCARD_STORAGE_ENABLED
342
// @Param: SD_MISSION
343
// @DisplayName: SDCard Mission size
344
// @Description: This sets the amount of storage in kilobytes reserved on the microsd card in mission.stg for waypoint storage. Each waypoint uses 15 bytes.
345
// @Range: 0 64
346
// @RebootRequired: True
347
// @User: Advanced
348
AP_GROUPINFO("SD_MISSION", 24, AP_BoardConfig, sdcard_storage.mission_kb, 0),
349
350
// @Param: SD_FENCE
351
// @DisplayName: SDCard Fence size
352
// @Description: This sets the amount of storage in kilobytes reserved on the microsd card in fence.stg for fence storage.
353
// @Range: 0 64
354
// @RebootRequired: True
355
// @User: Advanced
356
AP_GROUPINFO("SD_FENCE", 29, AP_BoardConfig, sdcard_storage.fence_kb, 0),
357
#endif
358
359
// index 25 used by SER5_RTSCTS
360
// index 26 used by SER3_RTSCTS
361
// index 27 used by SER4_RTSCTS
362
363
364
#if HAL_WITH_IO_MCU_DSHOT
365
// @Param: IO_DSHOT
366
// @DisplayName: Load DShot FW on IO
367
// @Description: This loads the DShot firmware on the IO co-processor
368
// @Values: 0:StandardFW,1:DshotFW
369
// @RebootRequired: True
370
// @User: Advanced
371
AP_GROUPINFO("IO_DSHOT", 28, AP_BoardConfig, state.io_dshot, 0),
372
#endif
373
AP_GROUPEND
374
};
375
376
void AP_BoardConfig::init()
377
{
378
// PARAMETER_CONVERSION - Added: APR-2022
379
vehicleSerialNumber.convert_parameter_width(AP_PARAM_INT16);
380
381
board_setup();
382
383
#if AP_RTC_ENABLED
384
AP::rtc().set_utc_usec(hal.util->get_hw_rtc(), AP_RTC::SOURCE_HW);
385
#endif
386
387
if (_boot_delay_ms > 0) {
388
uint16_t delay_ms = uint16_t(_boot_delay_ms.get());
389
if (hal.util->was_watchdog_armed() && delay_ms > 200) {
390
// don't delay a long time on watchdog reset, the pilot
391
// may be able to save the vehicle
392
delay_ms = 200;
393
}
394
hal.scheduler->delay(delay_ms);
395
}
396
397
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && defined(USE_POSIX)
398
uint8_t slowdown = constrain_int16(_sdcard_slowdown.get(), 0, 32);
399
const uint8_t max_slowdown = 8;
400
do {
401
if (AP::FS().retry_mount()) {
402
break;
403
}
404
slowdown++;
405
hal.scheduler->delay(5);
406
} while (slowdown < max_slowdown);
407
if (slowdown < max_slowdown) {
408
_sdcard_slowdown.set(slowdown);
409
} else {
410
printf("SDCard failed to start\n");
411
}
412
#endif
413
}
414
415
// set default value for BRD_SAFETY_MASK
416
void AP_BoardConfig::set_default_safety_ignore_mask(uint32_t mask)
417
{
418
state.ignore_safety_channels.set_default(mask);
419
}
420
421
void AP_BoardConfig::init_safety()
422
{
423
board_init_safety();
424
board_init_debug();
425
}
426
427
/*
428
notify user of a fatal startup error related to available sensors.
429
*/
430
bool AP_BoardConfig::_in_error_loop;
431
432
void AP_BoardConfig::throw_error(const char *err_type, const char *fmt, va_list arg)
433
{
434
_in_error_loop = true;
435
/*
436
to give the user the opportunity to connect to USB we keep
437
repeating the error. The mavlink delay callback is initialised
438
before this, so the user can change parameters (and in
439
particular BRD_TYPE if needed)
440
*/
441
uint32_t last_print_ms = 0;
442
while (true) {
443
uint32_t now = AP_HAL::millis();
444
if (now - last_print_ms >= 5000) {
445
last_print_ms = now;
446
char printfmt[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+2];
447
hal.util->snprintf(printfmt, sizeof(printfmt), "%s: %s\n", err_type, fmt);
448
{
449
va_list arg_copy;
450
va_copy(arg_copy, arg);
451
vprintf(printfmt, arg_copy);
452
va_end(arg_copy);
453
}
454
#if HAL_GCS_ENABLED
455
hal.util->snprintf(printfmt, sizeof(printfmt), "%s: %s", err_type, fmt);
456
{
457
va_list arg_copy;
458
va_copy(arg_copy, arg);
459
gcs().send_textv(MAV_SEVERITY_CRITICAL, printfmt, arg_copy);
460
va_end(arg_copy);
461
}
462
#endif
463
}
464
#if HAL_GCS_ENABLED
465
gcs().update_receive();
466
gcs().update_send();
467
#endif
468
EXPECT_DELAY_MS(10);
469
hal.scheduler->delay(5);
470
}
471
}
472
473
void AP_BoardConfig::allocation_error(const char *fmt, ...)
474
{
475
va_list arg_list;
476
va_start(arg_list, fmt);
477
char newfmt[64] {};
478
snprintf(newfmt, sizeof(newfmt), "Unable to allocate %s", fmt);
479
throw_error("Allocation Error", newfmt, arg_list);
480
va_end(arg_list);
481
}
482
483
void AP_BoardConfig::config_error(const char *fmt, ...)
484
{
485
va_list arg_list;
486
va_start(arg_list, fmt);
487
throw_error("Config Error", fmt, arg_list);
488
va_end(arg_list);
489
}
490
491
/*
492
handle logic for safety state button press. This should be called at
493
10Hz when the button is pressed. The button can either be directly
494
on a pin or on a UAVCAN device
495
This function returns true if the safety state should be toggled
496
*/
497
bool AP_BoardConfig::safety_button_handle_pressed(uint8_t press_count)
498
{
499
if (press_count != 10) {
500
return false;
501
}
502
// get button options
503
uint16_t safety_options = get_safety_button_options();
504
if (!(safety_options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) &&
505
hal.util->get_soft_armed()) {
506
return false;
507
}
508
AP_HAL::Util::safety_state safety_state = hal.util->safety_switch_state();
509
if (safety_state == AP_HAL::Util::SAFETY_DISARMED &&
510
!(safety_options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF)) {
511
return false;
512
}
513
if (safety_state == AP_HAL::Util::SAFETY_ARMED &&
514
!(safety_options & BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)) {
515
return false;
516
}
517
return true;
518
}
519
520
namespace AP {
521
AP_BoardConfig *boardConfig(void) {
522
return AP_BoardConfig::get_singleton();
523
}
524
};
525
526