/* setup sensors for Pixhawk2-slim */ void AP_BoardConfig::px4_start_pixhawk2slim_sensors(void) { #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) printf("Starting PH2SLIM sensors\n"); if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -I -R 4 start")) { printf("Have internal hmc5883\n"); } else { printf("No internal hmc5883\n"); } if (px4_start_driver(mpu9250_main, "mpu9250", "-R 14 start")) { printf("Found MPU9250 internal\n"); } else if (px4_start_driver(mpu6000_main, "mpu6000", "-R 14 -T 20608 start")) { printf("Found ICM20608 internal\n"); } else if (px4_start_driver(mpu6000_main, "mpu6000", "-R 14 start")) { printf("Found MPU6000 internal\n"); } else { px4_sensor_error("No MPU9250 or ICM20608 or MPU6000"); } // on Pixhawk2 default IMU temperature to 60 _imu_target_temperature.set_default(60); printf("PH2SLIM sensors started\n"); #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 }
/* setup optional sensors */ void AP_BoardConfig::vrx_start_optional_sensors(void) { if (px4_start_driver(ets_airspeed_main, "ets_airspeed", "start")) { printf("Found ETS airspeed sensor\n"); } else if (px4_start_driver(ets_airspeed_main, "meas_airspeed", "start -b 2")) { printf("Found ETS airspeed sensor (bus2)\n"); } if (px4_start_driver(meas_airspeed_main, "meas_airspeed", "start")) { printf("Found MEAS airspeed sensor\n"); } else if (px4_start_driver(meas_airspeed_main, "meas_airspeed", "start -b 2")) { printf("Found MEAS airspeed sensor (bus2)\n"); } if (px4_start_driver(ll40ls_main, "ll40ls", "-X start")) { printf("Found external ll40ls sensor\n"); } if (px4_start_driver(ll40ls_main, "ll40ls", "-I start")) { printf("Found internal ll40ls sensor\n"); } if (px4_start_driver(trone_main, "trone", "start")) { printf("Found trone sensor\n"); } if (px4_start_driver(mb12xx_main, "mb12xx", "start")) { printf("Found mb12xx sensor\n"); } if (px4_start_driver(pwm_input_main, "pwm_input", "start")) { printf("started pwm_input driver\n"); } }
/* setup common sensors */ void AP_BoardConfig::px4_start_common_sensors(void) { if (px4_start_driver(ms5611_main, "ms5611", "start")) { printf("ms5611 started OK\n"); } else { px4_sensor_error("no ms5611 found"); } if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -X start")) { printf("Have external hmc5883\n"); } else { printf("No external hmc5883\n"); } }
/* setup common sensors */ void AP_BoardConfig::vrx_start_common_sensors(void) { if (px4_start_driver(ms5611_main, "ms5611", "-s start")) { printf("MS5611 Internal started OK\n"); } else { px4_sensor_error("MS5611 Internal start failed"); } if (px4_start_driver(hmc5883_main, "hmc5883", "-C -X start")) { printf("HMC5883 External GPS started OK\n"); } else { printf("HMC5883 External GPS start failed\n"); } }
void AP_BoardConfig::vrx_start_brain54_sensors(void) { #if defined(CONFIG_ARCH_BOARD_VRBRAIN_V54) if (px4_start_driver(hmc5883_main, "hmc5883", "-C -R 12 -I start")) { printf("HMC5883 Internal GPS started OK\n"); } else { printf("HMC5883 Internal GPS start failed\n"); } if (px4_start_driver(mpu6000_main, "mpu6000", "-R 12 start")) { printf("MPU6000 Internal started OK\n"); } else { px4_sensor_error("MPU6000 Internal start failed"); } #endif }
/* setup sensors for PX4v1 */ void AP_BoardConfig::px4_start_fmuv1_sensors(void) { #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) printf("Starting FMUv1 sensors\n"); if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -I start")) { printf("Have internal hmc5883\n"); } else { printf("No internal hmc5883\n"); } if (px4_start_driver(mpu6000_main, "mpu6000", "start")) { printf("mpu6000 started OK\n"); } else { px4_sensor_error("mpu6000"); } px4.board_type.set_and_notify(PX4_BOARD_PX4V1); #endif // CONFIG_ARCH_BOARD_PX4FMU_V1 }
void AP_BoardConfig::vrx_start_core10_sensors(void) { #if defined(CONFIG_ARCH_BOARD_VRCORE_V10) if (px4_start_driver(mpu9250_main, "mpu9250", "-R 4 start")) { printf("MPU9250 Internal started OK\n"); } else { px4_sensor_error("MPU9250 Internal start failed"); } #endif }
void AP_BoardConfig::vrx_start_ubrain52_sensors(void) { #if defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52) if (px4_start_driver(mpu6000_main, "mpu6000", "-R 12 start")) { printf("MPU6000 Internal started OK\n"); } else { px4_sensor_error("MPU6000 Internal start failed"); } #endif }
/* setup sensors for FMUv4 */ void AP_BoardConfig::px4_start_fmuv4_sensors(void) { #if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) printf("Starting FMUv4 sensors\n"); if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -S -R 2 start")) { printf("Have SPI hmc5883\n"); } else { printf("No SPI hmc5883\n"); } if (px4_start_driver(mpu6000_main, "mpu6000", "-R 2 -T 20608 start")) { printf("Found ICM-20608 internal\n"); } if (px4_start_driver(mpu9250_main, "mpu9250", "-R 2 start")) { printf("Found mpu9250 internal\n"); } px4.board_type.set_and_notify(PX4_BOARD_PIXRACER); #endif // CONFIG_ARCH_BOARD_PX4FMU_V4 }
/* setup sensors for PHMINI */ void AP_BoardConfig::px4_start_phmini_sensors(void) { #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) printf("Starting PHMINI sensors\n"); // ICM20608 on SPI if (px4_start_driver(mpu6000_main, "mpu6000", "-S 2 -T 20608 start")) { printf("Found ICM20608 internal\n"); } else { px4_sensor_error("No ICM20608 found"); } if (px4_start_driver(mpu9250_main, "mpu9250", "start")) { printf("Found mpu9260\n"); } else { px4_sensor_error("No MPU9250 found"); } printf("PHMINI sensors started\n"); #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 }
void AP_BoardConfig::board_setup_drivers(void) { #if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) /* this works around an issue with some FMUv4 hardware (eg. copies of the Pixracer) which have incorrect components leading to sensor brownout on boot */ if (px4_start_driver(fmu_main, "fmu", "sensor_reset 20")) { printf("FMUv4 sensor reset complete\n"); } #endif if (state.board_type == PX4_BOARD_OLDDRIVERS) { printf("Old drivers no longer supported\n"); state.board_type = PX4_BOARD_AUTO; } // run board auto-detection board_autodetect(); if (state.board_type == PX4_BOARD_PH2SLIM || state.board_type == PX4_BOARD_PIXHAWK2) { _imu_target_temperature.set_default(45); if (_imu_target_temperature.get() < 0) { // don't allow a value of -1 on the cube, or it could cook // the IMU _imu_target_temperature.set(45); } } px4_configured_board = (enum px4_board_type)state.board_type.get(); switch (px4_configured_board) { case PX4_BOARD_PX4V1: case PX4_BOARD_PIXHAWK: case PX4_BOARD_PIXHAWK2: case PX4_BOARD_PIXRACER: case PX4_BOARD_PHMINI: case PX4_BOARD_AUAV21: case PX4_BOARD_PH2SLIM: case PX4_BOARD_AEROFC: case PX4_BOARD_PIXHAWK_PRO: case PX4_BOARD_PCNC1: case PX4_BOARD_MINDPXV2: break; default: sensor_config_error("Unknown board type"); break; } }
/* setup CANBUS drivers */ void AP_BoardConfig::px4_setup_canbus(void) { #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && !defined(CONFIG_ARCH_BOARD_PX4FMU_V1) if (px4.can_enable >= 1) { // give time for other drivers to fully start before we start // canbus. This prevents a race where a canbus mag comes up // before the hmc5883 hal.scheduler->delay(500); if (px4_start_driver(uavcan_main, "uavcan", "start")) { hal.console->printf("UAVCAN: started\n"); // give some time for CAN bus initialisation hal.scheduler->delay(2000); } else { hal.console->printf("UAVCAN: failed to start\n"); } // give time for canbus drivers to register themselves hal.scheduler->delay(1000); } if (px4.can_enable >= 2) { if (px4_start_driver(uavcan_main, "uavcan", "start fw")) { uint32_t start_wait_ms = AP_HAL::millis(); int fd = open("/dev/uavcan/esc", 0); // design flaw of uavcan driver, this should be /dev/uavcan/node one day if (fd == -1) { AP_HAL::panic("Configuration invalid - unable to open /dev/uavcan/esc"); } // delay startup, UAVCAN still discovering nodes while (ioctl(fd, UAVCAN_IOCG_NODEID_INPROGRESS,0) == OK && AP_HAL::millis() - start_wait_ms < 7000) { hal.scheduler->delay(500); } hal.console->printf("UAVCAN: node discovery complete\n"); close(fd); } } #endif // CONFIG_HAL_BOARD && !CONFIG_ARCH_BOARD_PX4FMU_V1 }
/* setup common sensors */ void AP_BoardConfig::px4_start_common_sensors(void) { #if defined(CONFIG_ARCH_BOARD_PX4FMU_V4) /* this works around an issue with some FMUv4 hardware (eg. copies of the Pixracer) which have incorrect components leading to sensor brownout on boot */ if (px4_start_driver(fmu_main, "fmu", "sensor_reset 20")) { printf("FMUv4 sensor reset complete\n"); } #endif if (px4_start_driver(ms5611_main, "ms5611", "start")) { printf("ms5611 started OK\n"); } else { px4_sensor_error("no ms5611 found"); } if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -X start")) { printf("Have external hmc5883\n"); } else { printf("No external hmc5883\n"); } }
/* setup sensors for PX4v2 */ void AP_BoardConfig::px4_start_fmuv2_sensors(void) { #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) bool have_FMUV3 = false; printf("Starting FMUv2 sensors\n"); if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -I -R 4 start")) { printf("Have internal hmc5883\n"); } else { printf("No internal hmc5883\n"); } // external MPU6000 is rotated YAW_180 from standard if (px4_start_driver(mpu6000_main, "mpu6000", "-X -R 4 start")) { printf("Found MPU6000 external\n"); have_FMUV3 = true; } else { if (px4_start_driver(mpu9250_main, "mpu9250", "-X -R 4 start")) { printf("Found MPU9250 external\n"); have_FMUV3 = true; } else { printf("No MPU6000 or MPU9250 external\n"); } } if (have_FMUV3) { // external L3GD20 is rotated YAW_180 from standard if (px4_start_driver(l3gd20_main, "l3gd20", "-X -R 4 start")) { printf("l3gd20 external started OK\n"); } else { px4_sensor_error("No l3gd20"); } // external LSM303D is rotated YAW_270 from standard if (px4_start_driver(lsm303d_main, "lsm303d", "-a 16 -X -R 6 start")) { printf("lsm303d external started OK\n"); } else { px4_sensor_error("No lsm303d"); } // internal MPU6000 is rotated ROLL_180_YAW_270 from standard if (px4_start_driver(mpu6000_main, "mpu6000", "-R 14 start")) { printf("Found MPU6000 internal\n"); } else { if (px4_start_driver(mpu9250_main, "mpu9250", "-R 14 start")) { printf("Found MPU9250 internal\n"); } else { px4_sensor_error("No MPU6000 or MPU9250"); } } if (px4_start_driver(hmc5883_main, "hmc5883", "-C -T -S -R 8 start")) { printf("Found SPI hmc5883\n"); } } else { // not FMUV3 (ie. not a pixhawk2) if (px4_start_driver(mpu6000_main, "mpu6000", "start")) { printf("Found MPU6000\n"); } else { if (px4_start_driver(mpu9250_main, "mpu9250", "start")) { printf("Found MPU9250\n"); } else { printf("No MPU6000 or MPU9250\n"); } } if (px4_start_driver(l3gd20_main, "l3gd20", "start")) { printf("l3gd20 started OK\n"); } else { px4_sensor_error("no l3gd20 found"); } if (px4_start_driver(lsm303d_main, "lsm303d", "-a 16 start")) { printf("lsm303d started OK\n"); } else { px4_sensor_error("no lsm303d found"); } } if (have_FMUV3) { // on Pixhawk2 default IMU temperature to 60 _imu_target_temperature.set_default(60); px4.board_type.set_and_notify(PX4_BOARD_PIXHAWK2); } else { px4.board_type.set_and_notify(PX4_BOARD_PIXHAWK); } printf("FMUv2 sensors started\n"); #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 }