Line data Source code
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 - driver loading and setup
17 : */
18 :
19 :
20 : #include <AP_HAL/AP_HAL.h>
21 : #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
22 : #include <hal.h>
23 : #endif
24 : #include "AP_BoardConfig.h"
25 : #include <GCS_MAVLink/GCS.h>
26 : #include <AP_Math/crc.h>
27 : #include <stdio.h>
28 :
29 : extern const AP_HAL::HAL& hal;
30 :
31 : /*
32 : init safety state
33 : */
34 2086 : void AP_BoardConfig::board_init_safety()
35 : {
36 2086 : bool force_safety_off = (state.safety_enable.get() == 0);
37 2086 : if (!force_safety_off && hal.util->was_watchdog_safety_off()) {
38 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Forcing safety off for watchdog");
39 0 : force_safety_off = true;
40 : }
41 2086 : if (force_safety_off) {
42 2086 : hal.rcout->force_safety_off();
43 : // wait until safety has been turned off
44 2086 : uint8_t count = 20;
45 2086 : while (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED && count--) {
46 0 : hal.scheduler->delay(20);
47 : }
48 : }
49 2086 : }
50 :
51 : /*
52 : init debug pins. We set debug pins as input if BRD_OPTIONS bit for debug enable is not set
53 : this prevents possible ESD issues on the debug pins
54 : */
55 2086 : void AP_BoardConfig::board_init_debug()
56 : {
57 : #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_DEBUG_BUILD)
58 : if ((_options & BOARD_OPTION_DEBUG_ENABLE) == 0) {
59 : #ifdef HAL_GPIO_PIN_JTCK_SWCLK
60 : palSetLineMode(HAL_GPIO_PIN_JTCK_SWCLK, PAL_MODE_INPUT);
61 : #endif
62 : #ifdef HAL_GPIO_PIN_JTMS_SWDIO
63 : palSetLineMode(HAL_GPIO_PIN_JTMS_SWDIO, PAL_MODE_INPUT);
64 : #endif
65 : }
66 : #endif // HAL_BUILD_AP_PERIPH && HAL_DEBUG_BUILD
67 2086 : }
68 :
69 :
70 : #if AP_FEATURE_BOARD_DETECT
71 :
72 : AP_BoardConfig::px4_board_type AP_BoardConfig::px4_configured_board;
73 :
74 : void AP_BoardConfig::board_setup_drivers(void)
75 : {
76 : if (state.board_type == PX4_BOARD_OLDDRIVERS) {
77 : printf("Old drivers no longer supported\n");
78 : state.board_type.set(PX4_BOARD_AUTO);
79 : }
80 :
81 : // run board auto-detection
82 : board_autodetect();
83 :
84 : px4_configured_board = (enum px4_board_type)state.board_type.get();
85 :
86 : switch (px4_configured_board) {
87 : case PX4_BOARD_PX4V1:
88 : case PX4_BOARD_PIXHAWK:
89 : case PX4_BOARD_PIXHAWK2:
90 : case PX4_BOARD_FMUV6:
91 : case PX4_BOARD_PHMINI:
92 : case PX4_BOARD_AUAV21:
93 : case PX4_BOARD_PH2SLIM:
94 : case PX4_BOARD_AEROFC:
95 : case FMUV6_BOARD_HOLYBRO_6X:
96 : case FMUV6_BOARD_HOLYBRO_6X_REV6:
97 : case FMUV6_BOARD_HOLYBRO_6X_45686:
98 : case FMUV6_BOARD_CUAV_6X:
99 : break;
100 : default:
101 : config_error("Unknown board type %u", px4_configured_board);
102 : break;
103 : }
104 : }
105 :
106 : #define SPI_PROBE_DEBUG 0
107 :
108 : /*
109 : check a SPI device for a register value
110 : */
111 : bool AP_BoardConfig::spi_check_register(const char *devname, uint8_t regnum, uint8_t value, uint8_t read_flag)
112 : {
113 : auto dev = hal.spi->get_device(devname);
114 : if (!dev) {
115 : #if SPI_PROBE_DEBUG
116 : hal.console->printf("%s: no device\n", devname);
117 : #endif
118 : return false;
119 : }
120 : dev->set_read_flag(read_flag);
121 : WITH_SEMAPHORE(dev->get_semaphore());
122 : dev->set_speed(AP_HAL::Device::SPEED_LOW);
123 : uint8_t v;
124 : if (!dev->read_registers(regnum, &v, 1)) {
125 : #if SPI_PROBE_DEBUG
126 : hal.console->printf("%s: reg %02x read fail\n", devname, (unsigned)regnum);
127 : #endif
128 : return false;
129 : }
130 : #if SPI_PROBE_DEBUG
131 : hal.console->printf("%s: reg %02x expected:%02x got:%02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v);
132 : #endif
133 : return v == value;
134 : }
135 :
136 :
137 : #define INV2REG_BANK_SEL 0x7F
138 : /*
139 : check a SPI device for a register value
140 : */
141 : bool AP_BoardConfig::spi_check_register_inv2(const char *devname, uint8_t regnum, uint8_t value, uint8_t read_flag)
142 : {
143 : auto dev = hal.spi->get_device(devname);
144 : if (!dev) {
145 : #if SPI_PROBE_DEBUG
146 : hal.console->printf("%s: no device\n", devname);
147 : #endif
148 : return false;
149 : }
150 : dev->set_read_flag(read_flag);
151 : WITH_SEMAPHORE(dev->get_semaphore());
152 : dev->set_speed(AP_HAL::Device::SPEED_LOW);
153 : uint8_t v;
154 : // select bank 0 for who am i
155 : dev->write_register(INV2REG_BANK_SEL, 0, false);
156 : if (!dev->read_registers(regnum, &v, 1)) {
157 : #if SPI_PROBE_DEBUG
158 : hal.console->printf("%s: reg %02x read fail\n", devname, (unsigned)regnum);
159 : #endif
160 : return false;
161 : }
162 : #if SPI_PROBE_DEBUG
163 : hal.console->printf("%s: reg %02x expected:%02x got:%02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v);
164 : #endif
165 : return v == value;
166 : }
167 :
168 : #if defined(HAL_VALIDATE_BOARD)
169 : bool AP_BoardConfig::check_ms5611(const char* devname) {
170 : auto dev = hal.spi->get_device(devname);
171 : if (!dev) {
172 : #if SPI_PROBE_DEBUG
173 : hal.console->printf("%s: no device\n", devname);
174 : #endif
175 : return false;
176 : }
177 :
178 : AP_HAL::Semaphore *dev_sem = dev->get_semaphore();
179 :
180 : if (!dev_sem) {
181 : return false;
182 : }
183 : WITH_SEMAPHORE(dev_sem);
184 :
185 : static const uint8_t CMD_MS56XX_RESET = 0x1E;
186 : static const uint8_t CMD_MS56XX_PROM = 0xA0;
187 :
188 : dev->transfer(&CMD_MS56XX_RESET, 1, nullptr, 0);
189 : hal.scheduler->delay(4);
190 :
191 : uint16_t prom[8];
192 : bool all_zero = true;
193 : for (uint8_t i = 0; i < 8; i++) {
194 : const uint8_t reg = CMD_MS56XX_PROM + (i << 1);
195 : uint8_t val[2];
196 : if (!dev->transfer(®, 1, val, sizeof(val))) {
197 : #if SPI_PROBE_DEBUG
198 : hal.console->printf("%s: transfer fail\n", devname);
199 : #endif
200 : return false;
201 : }
202 : prom[i] = (val[0] << 8) | val[1];
203 :
204 : if (prom[i] != 0) {
205 : all_zero = false;
206 : }
207 : }
208 :
209 : uint16_t crc_read = prom[7]&0xf;
210 : prom[7] &= 0xff00;
211 :
212 : if (crc_read != crc_crc4(prom) || all_zero) {
213 : #if SPI_PROBE_DEBUG
214 : hal.console->printf("%s: crc fail\n", devname);
215 : #endif
216 : return false;
217 : }
218 :
219 : #if SPI_PROBE_DEBUG
220 : hal.console->printf("%s: found successfully\n", devname);
221 : #endif
222 :
223 : return true;
224 : }
225 : #endif // HAL_VALIDATE_BOARD
226 :
227 : #define MPUREG_WHOAMI 0x75
228 : #define MPU_WHOAMI_MPU60X0 0x68
229 : #define MPU_WHOAMI_MPU9250 0x71
230 : #define MPU_WHOAMI_ICM20608 0xaf
231 : #define MPU_WHOAMI_ICM20602 0x12
232 :
233 : #define LSMREG_WHOAMI 0x0f
234 : #define LSM_WHOAMI_LSM303D 0x49
235 : #define LSM_WHOAMI_L3GD20 0xd4
236 :
237 : #define INV2REG_WHOAMI 0x00
238 :
239 : #define INV2_WHOAMI_ICM20948 0xEA
240 : #define INV2_WHOAMI_ICM20649 0xE1
241 :
242 : #define INV3REG_WHOAMI 0x75
243 : #define INV3REG_456_WHOAMI 0x72
244 :
245 : #define INV3_WHOAMI_ICM42688 0x47
246 : #define INV3_WHOAMI_ICM42670 0x67
247 : #define INV3_WHOAMI_ICM45686 0xE9
248 : #define INV3_WHOAMI_IIM42652 0x6f
249 :
250 : /*
251 : validation of the board type
252 : */
253 : void AP_BoardConfig::validate_board_type(void)
254 : {
255 : /* some boards can be damaged by the user setting the wrong board
256 : type. The key one is the cube which has a heater which can
257 : cook the IMUs if the user uses an old paramater file. We
258 : override the board type for that specific case
259 : */
260 : #if defined(HAL_CHIBIOS_ARCH_FMUV3)
261 : if (state.board_type == PX4_BOARD_PIXHAWK &&
262 : (spi_check_register("mpu6000_ext", MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
263 : spi_check_register("mpu9250_ext", MPUREG_WHOAMI, MPU_WHOAMI_MPU9250) ||
264 : spi_check_register("icm20608", MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
265 : spi_check_register("icm20608_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602) ||
266 : spi_check_register("icm20602_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602)) &&
267 : (spi_check_register("lsm9ds0_ext_am", LSMREG_WHOAMI, LSM_WHOAMI_LSM303D) ||
268 : spi_check_register_inv2("icm20948_ext", INV2REG_WHOAMI, INV2_WHOAMI_ICM20948))) {
269 : // Pixhawk2 has LSM303D and MPUxxxx on external bus. If we
270 : // detect those, then force PIXHAWK2, even if the user has
271 : // configured for PIXHAWK1
272 : #if !defined(HAL_CHIBIOS_ARCH_FMUV3)
273 : // force user to load the right firmware
274 : config_error("Pixhawk2 requires FMUv3 firmware");
275 : #endif
276 : state.board_type.set(PX4_BOARD_PIXHAWK2);
277 : DEV_PRINTF("Forced PIXHAWK2\n");
278 : }
279 : #endif
280 : }
281 :
282 : /*
283 : auto-detect board type
284 : */
285 : void AP_BoardConfig::board_autodetect(void)
286 : {
287 : #if defined(HAL_VALIDATE_BOARD)
288 : if((_options & SKIP_BOARD_VALIDATION) == 0) {
289 : const char* errored_check = HAL_VALIDATE_BOARD;
290 : if (errored_check == nullptr) {
291 : return;
292 : } else {
293 : config_error("Board Validation %s Failed", errored_check);
294 : return;
295 : }
296 : }
297 : #endif
298 :
299 : if (state.board_type != PX4_BOARD_AUTO) {
300 : validate_board_type();
301 : // user has chosen a board type
302 : return;
303 : }
304 :
305 : #if defined(HAL_CHIBIOS_ARCH_FMUV3)
306 : if ((spi_check_register("mpu6000_ext", MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
307 : spi_check_register("mpu6000_ext", MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
308 : spi_check_register("mpu9250_ext", MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
309 : spi_check_register("mpu9250_ext", MPUREG_WHOAMI, MPU_WHOAMI_MPU9250) ||
310 : spi_check_register("icm20608_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
311 : spi_check_register("icm20608_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602) ||
312 : spi_check_register("icm20602_ext", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602)) &&
313 : (spi_check_register("lsm9ds0_ext_am", LSMREG_WHOAMI, LSM_WHOAMI_LSM303D) ||
314 : spi_check_register_inv2("icm20948_ext", INV2REG_WHOAMI, INV2_WHOAMI_ICM20948))) {
315 : // Pixhawk2 has LSM303D and MPUxxxx on external bus
316 : state.board_type.set(PX4_BOARD_PIXHAWK2);
317 : DEV_PRINTF("Detected PIXHAWK2\n");
318 : } else if ((spi_check_register("icm20608-am", MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
319 : spi_check_register("icm20608-am", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602)) &&
320 : spi_check_register("mpu9250", MPUREG_WHOAMI, MPU_WHOAMI_MPU9250)) {
321 : // PHMINI has an ICM20608 and MPU9250 on sensor bus
322 : state.board_type.set(PX4_BOARD_PHMINI);
323 : DEV_PRINTF("Detected PixhawkMini\n");
324 : } else if (spi_check_register("lsm9ds0_am", LSMREG_WHOAMI, LSM_WHOAMI_LSM303D) &&
325 : (spi_check_register("mpu6000", MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
326 : spi_check_register("icm20608", MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
327 : spi_check_register("icm20608", MPUREG_WHOAMI, MPU_WHOAMI_ICM20602) ||
328 : spi_check_register("mpu9250", MPUREG_WHOAMI, MPU_WHOAMI_MPU9250))) {
329 :
330 : // classic or upgraded Pixhawk1
331 : state.board_type.set(PX4_BOARD_PIXHAWK);
332 : DEV_PRINTF("Detected Pixhawk\n");
333 : } else {
334 : config_error("Unable to detect board type");
335 : }
336 : #elif defined(HAL_CHIBIOS_ARCH_FMUV6)
337 : detect_fmuv6_variant();
338 : #endif
339 :
340 : }
341 :
342 : #endif // AP_FEATURE_BOARD_DETECT
343 :
344 : /*
345 : setup flow control on UARTs
346 : */
347 2094 : void AP_BoardConfig::board_setup_uart()
348 : {
349 : #if AP_FEATURE_RTSCTS
350 : #ifdef HAL_HAVE_RTSCTS_SERIAL1
351 2094 : if (hal.serial(1) != nullptr) {
352 2094 : hal.serial(1)->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser_rtscts[1].get());
353 : }
354 : #endif
355 : #ifdef HAL_HAVE_RTSCTS_SERIAL2
356 2094 : if (hal.serial(2) != nullptr) {
357 2094 : hal.serial(2)->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser_rtscts[2].get());
358 : }
359 : #endif
360 : #ifdef HAL_HAVE_RTSCTS_SERIAL3
361 2094 : if (hal.serial(3) != nullptr) {
362 2094 : hal.serial(3)->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser_rtscts[3].get());
363 : }
364 : #endif
365 : #ifdef HAL_HAVE_RTSCTS_SERIAL4
366 2094 : if (hal.serial(4) != nullptr) {
367 2094 : hal.serial(4)->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser_rtscts[4].get());
368 : }
369 : #endif
370 : #ifdef HAL_HAVE_RTSCTS_SERIAL5
371 2094 : if (hal.serial(5) != nullptr) {
372 2094 : hal.serial(5)->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser_rtscts[5].get());
373 : }
374 : #endif
375 : #ifdef HAL_HAVE_RTSCTS_SERIAL6
376 2094 : if (hal.serial(6) != nullptr) {
377 2094 : hal.serial(6)->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser_rtscts[6].get());
378 : }
379 : #endif
380 : #ifdef HAL_HAVE_RTSCTS_SERIAL7
381 2094 : if (hal.serial(7) != nullptr) {
382 2094 : hal.serial(7)->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser_rtscts[7].get());
383 : }
384 : #endif
385 : #ifdef HAL_HAVE_RTSCTS_SERIAL8
386 2094 : if (hal.serial(8) != nullptr) {
387 2094 : hal.serial(8)->set_flow_control((AP_HAL::UARTDriver::flow_control)state.ser_rtscts[8].get());
388 : }
389 : #endif
390 : #endif
391 2094 : }
392 :
393 : /*
394 : setup SBUS
395 : */
396 2094 : void AP_BoardConfig::board_setup_sbus(void)
397 : {
398 : #if AP_FEATURE_SBUS_OUT
399 : if (state.sbus_out_rate.get() >= 1) {
400 : static const struct {
401 : uint8_t value;
402 : uint16_t rate;
403 : } rates[] = {
404 : { 1, 50 },
405 : { 2, 75 },
406 : { 3, 100 },
407 : { 4, 150 },
408 : { 5, 200 },
409 : { 6, 250 },
410 : { 7, 300 }
411 : };
412 : uint16_t rate = 300;
413 : for (uint8_t i=0; i<ARRAY_SIZE(rates); i++) {
414 : if (rates[i].value == state.sbus_out_rate) {
415 : rate = rates[i].rate;
416 : }
417 : }
418 : if (!hal.rcout->enable_px4io_sbus_out(rate)) {
419 : hal.console->printf("Failed to enable SBUS out\n");
420 : }
421 : }
422 : #endif
423 2094 : }
424 :
425 :
426 : /*
427 : setup peripherals and drivers
428 : */
429 2094 : void AP_BoardConfig::board_setup()
430 : {
431 : #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
432 : // init needs to be done after boardconfig is read so parameters are set
433 : hal.gpio->init();
434 : hal.rcin->init();
435 : hal.rcout->init();
436 : #endif
437 :
438 : #ifdef HAL_GPIO_PWM_VOLT_PIN
439 : if (_pwm_volt_sel == 0) {
440 : hal.gpio->write(HAL_GPIO_PWM_VOLT_PIN, HAL_GPIO_PWM_VOLT_3v3); //set pin for 3.3V PWM Output
441 : } else if (_pwm_volt_sel == 1) {
442 : hal.gpio->write(HAL_GPIO_PWM_VOLT_PIN, !HAL_GPIO_PWM_VOLT_3v3); //set pin for 5V PWM Output
443 : }
444 : #endif
445 2094 : board_setup_uart();
446 2094 : board_setup_sbus();
447 : #if AP_FEATURE_BOARD_DETECT
448 : board_setup_drivers();
449 : #endif
450 2094 : }
451 :
452 :
453 : #ifdef HAL_CHIBIOS_ARCH_FMUV6
454 :
455 : #define BMI088REG_CHIPID 0x00
456 : #define CHIPID_BMI088_G 0x0F
457 :
458 : /*
459 : detect which FMUV6 variant we are running on
460 : */
461 : void AP_BoardConfig::detect_fmuv6_variant()
462 : {
463 : if (((spi_check_register_inv2("icm20649", INV2REG_WHOAMI, INV2_WHOAMI_ICM20649) ||
464 : spi_check_register("bmi088_g", BMI088REG_CHIPID, CHIPID_BMI088_G)) && // alternative config
465 : spi_check_register("icm42688", INV3REG_WHOAMI, INV3_WHOAMI_ICM42688) &&
466 : spi_check_register("icm42670", INV3REG_WHOAMI, INV3_WHOAMI_ICM42670))) {
467 : state.board_type.set_and_notify(FMUV6_BOARD_HOLYBRO_6X);
468 : DEV_PRINTF("Detected Holybro 6X\n");
469 : } else if ((spi_check_register_inv2("icm20649_2", INV2REG_WHOAMI, INV2_WHOAMI_ICM20649) &&
470 : spi_check_register("icm42688", INV3REG_WHOAMI, INV3_WHOAMI_ICM42688) &&
471 : spi_check_register("bmi088_g", BMI088REG_CHIPID, CHIPID_BMI088_G))) {
472 : state.board_type.set_and_notify(FMUV6_BOARD_CUAV_6X);
473 : DEV_PRINTF("Detected CUAV 6X\n");
474 : AP_Param::load_defaults_file("@ROMFS/param/CUAV_V6X_defaults.parm", false);
475 : } else if (spi_check_register("icm45686-1", INV3REG_456_WHOAMI, INV3_WHOAMI_ICM45686) &&
476 : spi_check_register("icm45686-2", INV3REG_456_WHOAMI, INV3_WHOAMI_ICM45686) &&
477 : spi_check_register("icm45686-3", INV3REG_456_WHOAMI, INV3_WHOAMI_ICM45686)) {
478 : state.board_type.set_and_notify(FMUV6_BOARD_HOLYBRO_6X_45686);
479 : DEV_PRINTF("Detected Holybro 6X_45686\n");
480 : } else if (spi_check_register("iim42652", INV3REG_WHOAMI, INV3_WHOAMI_IIM42652) &&
481 : spi_check_register("icm45686", INV3REG_456_WHOAMI, INV3_WHOAMI_ICM45686)) {
482 : state.board_type.set_and_notify(FMUV6_BOARD_HOLYBRO_6X_REV6);
483 : DEV_PRINTF("Detected Holybro 6X_Rev6\n");
484 : }
485 : }
486 : #endif
|
