ardupilot/libraries/AP_BoardConfig/px4_drivers.cpp

539 lines
16 KiB
C++

/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* AP_BoardConfig - px4 driver loading and setup
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_BoardConfig.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <GCS_MAVLink/GCS.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_sbus.h>
#include <nuttx/arch.h>
#include <spawn.h>
extern const AP_HAL::HAL& hal;
AP_BoardConfig::px4_board_type AP_BoardConfig::px4_configured_board;
/*
declare driver main entry points
*/
extern "C" {
#if HAL_WITH_UAVCAN
int uavcan_main(int, char **);
#endif
int fmu_main(int, char **);
int px4io_main(int, char **);
int adc_main(int, char **);
int tone_alarm_main(int, char **);
};
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 && !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
/*
this is needed for the code to wait for CAN startup
*/
#define _UAVCAN_IOCBASE (0x4000) // IOCTL base for module UAVCAN
#define _UAVCAN_IOC(_n) (_IOC(_UAVCAN_IOCBASE, _n))
#define UAVCAN_IOCG_NODEID_INPROGRESS _UAVCAN_IOC(1) // query if node identification is in progress
#endif
/*
setup PWM pins
*/
void AP_BoardConfig::px4_setup_pwm()
{
/* configure the FMU driver for the right number of PWMs */
static const struct {
uint8_t mode_parm;
uint8_t mode_value;
uint8_t num_gpios;
} mode_table[] = {
/* table mapping BRD_PWM_COUNT to ioctl arguments */
{ 0, PWM_SERVO_MODE_NONE, 6 },
{ 2, PWM_SERVO_MODE_2PWM, 4 },
{ 4, PWM_SERVO_MODE_4PWM, 2 },
{ 6, PWM_SERVO_MODE_6PWM, 0 },
{ 7, PWM_SERVO_MODE_3PWM1CAP, 2 },
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
{ 8, PWM_SERVO_MODE_12PWM, 0 },
#endif
};
uint8_t mode_parm = (uint8_t)px4.pwm_count.get();
uint8_t i;
for (i=0; i<ARRAY_SIZE(mode_table); i++) {
if (mode_table[i].mode_parm == mode_parm) {
break;
}
}
if (i == ARRAY_SIZE(mode_table)) {
hal.console->printf("RCOutput: invalid BRD_PWM_COUNT %u\n", mode_parm);
} else {
int fd = open("/dev/px4fmu", 0);
if (fd == -1) {
AP_HAL::panic("Unable to open /dev/px4fmu");
}
if (ioctl(fd, PWM_SERVO_SET_MODE, mode_table[i].mode_value) != 0) {
hal.console->printf("RCOutput: unable to setup AUX PWM with BRD_PWM_COUNT %u\n", mode_parm);
}
close(fd);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
if (mode_table[i].num_gpios < 2) {
// reduce change of config mistake where relay and PWM interfere
AP_Param::set_default_by_name("RELAY_PIN", -1);
AP_Param::set_default_by_name("RELAY_PIN2", -1);
}
#endif
}
}
/*
setup flow control on UARTs
*/
void AP_BoardConfig::px4_setup_uart()
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
hal.uartC->set_flow_control((AP_HAL::UARTDriver::flow_control)px4.ser1_rtscts.get());
if (hal.uartD != nullptr) {
hal.uartD->set_flow_control((AP_HAL::UARTDriver::flow_control)px4.ser2_rtscts.get());
}
#endif
}
/*
setup safety switch
*/
void AP_BoardConfig::px4_setup_safety_mask()
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
// setup channels to ignore the armed state
int px4io_fd = open("/dev/px4io", 0);
if (px4io_fd != -1) {
if (ioctl(px4io_fd, PWM_SERVO_IGNORE_SAFETY, (uint16_t)px4.ignore_safety_channels) != 0) {
hal.console->printf("IGNORE_SAFETY failed\n");
}
}
int px4fmu_fd = open("/dev/px4fmu", 0);
if (px4fmu_fd != -1) {
uint16_t mask = px4.ignore_safety_channels;
if (px4io_fd != -1) {
mask >>= 8;
}
if (ioctl(px4fmu_fd, PWM_SERVO_IGNORE_SAFETY, (uint16_t)mask) != 0) {
hal.console->printf("IGNORE_SAFETY failed\n");
}
close(px4fmu_fd);
}
if (px4io_fd != -1) {
close(px4io_fd);
}
#endif
}
/*
setup safety switch
*/
void AP_BoardConfig::px4_setup_safety()
{
px4_setup_safety_mask();
if (px4.safety_enable.get() == 0) {
hal.rcout->force_safety_off();
hal.rcout->force_safety_no_wait();
// wait until safety has been turned off
uint8_t count = 20;
while (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_ARMED && count--) {
hal.scheduler->delay(20);
}
}
}
/*
setup SBUS
*/
void AP_BoardConfig::px4_setup_sbus(void)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
if (px4.sbus_out_rate.get() >= 1) {
static const struct {
uint8_t value;
uint16_t rate;
} rates[] = {
{ 1, 50 },
{ 2, 75 },
{ 3, 100 },
{ 4, 150 },
{ 5, 200 },
{ 6, 250 },
{ 7, 300 }
};
uint16_t rate = 300;
for (uint8_t i=0; i<ARRAY_SIZE(rates); i++) {
if (rates[i].value == px4.sbus_out_rate) {
rate = rates[i].rate;
}
}
if (!hal.rcout->enable_sbus_out(rate)) {
hal.console->printf("Failed to enable SBUS out\n");
}
}
#endif
}
/*
setup CANBUS drivers
*/
void AP_BoardConfig::px4_setup_canbus(void)
{
#if HAL_WITH_UAVCAN
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(2000);
}
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
}
extern "C" int waitpid(pid_t, int *, int);
/*
start one px4 driver
*/
bool AP_BoardConfig::px4_start_driver(main_fn_t main_function, const char *name, const char *arguments)
{
char *s = strdup(arguments);
char *args[10];
uint8_t nargs = 0;
char *saveptr = nullptr;
// parse into separate arguments
for (char *tok=strtok_r(s, " ", &saveptr); tok; tok=strtok_r(nullptr, " ", &saveptr)) {
args[nargs++] = tok;
if (nargs == ARRAY_SIZE(args)-1) {
break;
}
}
args[nargs++] = nullptr;
printf("Starting driver %s %s\n", name, arguments);
pid_t pid;
if (task_spawn(&pid, name, main_function, nullptr, nullptr,
args, nullptr) != 0) {
free(s);
printf("Failed to spawn %s\n", name);
return false;
}
// wait for task to exit and gather status
int status = -1;
if (waitpid(pid, &status, 0) != pid) {
printf("waitpid failed for %s\n", name);
free(s);
return false;
}
free(s);
return (status >> 8) == 0;
}
void AP_BoardConfig::px4_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 (px4.board_type == PX4_BOARD_OLDDRIVERS) {
printf("Old drivers no longer supported\n");
px4.board_type = PX4_BOARD_AUTO;
}
// run board auto-detection
px4_autodetect();
if (px4.board_type == PX4_BOARD_PH2SLIM ||
px4.board_type == PX4_BOARD_PIXHAWK2) {
_imu_target_temperature.set_default(60);
}
px4_configured_board = (enum px4_board_type)px4.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_PH2SLIM:
break;
default:
px4_sensor_error("Unknown board type");
break;
}
}
/*
play a tune
*/
void AP_BoardConfig::px4_tone_alarm(const char *tone_string)
{
px4_start_driver(tone_alarm_main, "tone_alarm", tone_string);
}
/*
setup px4io, possibly updating firmware
*/
void AP_BoardConfig::px4_setup_px4io(void)
{
if (px4_start_driver(px4io_main, "px4io", "start norc")) {
printf("px4io started OK\n");
} else {
// might be in bootloader mode if user held down safety switch
// at power on
printf("Loading /etc/px4io/px4io.bin\n");
px4_tone_alarm("MBABGP");
if (px4_start_driver(px4io_main, "px4io", "update /etc/px4io/px4io.bin")) {
printf("upgraded PX4IO firmware OK\n");
px4_tone_alarm("MSPAA");
} else {
printf("Failed to upgrade PX4IO firmware\n");
px4_tone_alarm("MNGGG");
}
hal.scheduler->delay(1000);
if (px4_start_driver(px4io_main, "px4io", "start norc")) {
printf("px4io started OK\n");
} else {
px4_sensor_error("px4io start failed");
}
}
/*
see if we need to update px4io firmware
*/
if (px4_start_driver(px4io_main, "px4io", "checkcrc /etc/px4io/px4io.bin")) {
printf("PX4IO CRC OK\n");
} else {
printf("PX4IO CRC failure\n");
px4_tone_alarm("MBABGP");
if (px4_start_driver(px4io_main, "px4io", "safety_on")) {
printf("PX4IO disarm OK\n");
} else {
printf("PX4IO disarm failed\n");
}
hal.scheduler->delay(1000);
if (px4_start_driver(px4io_main, "px4io", "forceupdate 14662 /etc/px4io/px4io.bin")) {
hal.scheduler->delay(1000);
if (px4_start_driver(px4io_main, "px4io", "start norc")) {
printf("px4io restart OK\n");
px4_tone_alarm("MSPAA");
} else {
px4_tone_alarm("MNGGG");
px4_sensor_error("PX4IO restart failed");
}
} else {
printf("PX4IO update failed\n");
px4_tone_alarm("MNGGG");
}
}
}
/*
setup required peripherals like adc, rcinput and rcoutput
*/
void AP_BoardConfig::px4_setup_peripherals(void)
{
// always start adc
if (px4_start_driver(adc_main, "adc", "start")) {
hal.analogin->init();
printf("ADC started OK\n");
} else {
px4_sensor_error("no ADC found");
}
#ifndef CONFIG_ARCH_BOARD_PX4FMU_V4
px4_setup_px4io();
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
const char *fmu_mode = "mode_serial";
#else
const char *fmu_mode = "mode_pwm4";
#endif
if (px4_start_driver(fmu_main, "fmu", fmu_mode)) {
printf("fmu %s started OK\n", fmu_mode);
} else {
px4_sensor_error("fmu start failed");
}
hal.gpio->init();
hal.rcin->init();
hal.rcout->init();
}
/*
check a SPI device for a register value
*/
bool AP_BoardConfig::spi_check_register(const char *devname, uint8_t regnum, uint8_t value, uint8_t read_flag)
{
auto dev = hal.spi->get_device(devname);
if (!dev) {
printf("%s: no device\n", devname);
return false;
}
dev->set_read_flag(read_flag);
uint8_t v;
if (!dev->read_registers(regnum, &v, 1)) {
printf("%s: reg %02x read fail\n", devname, (unsigned)regnum);
return false;
}
printf("%s: reg %02x %02x %02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v);
return v == value;
}
#define MPUREG_WHOAMI 0x75
#define MPU_WHOAMI_MPU60X0 0x68
#define MPU_WHOAMI_MPU9250 0x71
#define MPU_WHOAMI_ICM20608 0xaf
#define LSMREG_WHOAMI 0x0f
#define LSM_WHOAMI_LSM303D 0x49
/*
auto-detect board type
*/
void AP_BoardConfig::px4_autodetect(void)
{
if (px4.board_type != PX4_BOARD_AUTO) {
// user has chosen a board type
return;
}
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
// only one choice
px4.board_type.set(PX4_BOARD_PX4V1);
hal.console->printf("Detected PX4v1\n");
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
if ((spi_check_register(HAL_INS_MPU60x0_EXT_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
spi_check_register(HAL_INS_MPU9250_EXT_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU9250) ||
spi_check_register(HAL_INS_ICM20608_EXT_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20608)) &&
spi_check_register(HAL_INS_LSM9DS0_EXT_A_NAME, LSMREG_WHOAMI, LSM_WHOAMI_LSM303D)) {
// Pixhawk2 has LSM303D and MPUxxxx on external bus
px4.board_type.set(PX4_BOARD_PIXHAWK2);
hal.console->printf("Detected PIXHAWK2\n");
} else if (spi_check_register(HAL_INS_ICM20608_AM_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) &&
spi_check_register(HAL_INS_MPU9250_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU9250)) {
// PHMINI has an ICM20608 and MPU9250 on sensor bus
px4.board_type.set(PX4_BOARD_PHMINI);
hal.console->printf("Detected PixhawkMini\n");
} else if (spi_check_register(HAL_INS_LSM9DS0_A_NAME, LSMREG_WHOAMI, LSM_WHOAMI_LSM303D) &&
(spi_check_register(HAL_INS_MPU60x0_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU60X0) ||
spi_check_register(HAL_INS_ICM20608_NAME, MPUREG_WHOAMI, MPU_WHOAMI_ICM20608) ||
spi_check_register(HAL_INS_MPU9250_NAME, MPUREG_WHOAMI, MPU_WHOAMI_MPU9250))) {
// classic or upgraded Pixhawk1
px4.board_type.set(PX4_BOARD_PIXHAWK);
hal.console->printf("Detected Pixhawk\n");
} else {
px4_sensor_error("Unable to detect board type");
}
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
// only one choice
px4.board_type.set_and_notify(PX4_BOARD_PIXRACER);
hal.console->printf("Detected Pixracer\n");
#endif
}
/*
fail startup of a required sensor
*/
void AP_BoardConfig::px4_sensor_error(const char *reason)
{
/*
to give the user the opportunity to connect to USB we keep
repeating the error. The mavlink delay callback is initialised
before this, so the user can change parameters (and in
particular BRD_TYPE if needed)
*/
while (true) {
printf("Sensor failure: %s\n", reason);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_ERROR, "Check BRD_TYPE: %s", reason);
hal.scheduler->delay(3000);
}
}
/*
setup px4 peripherals and drivers
*/
void AP_BoardConfig::px4_setup()
{
px4_setup_peripherals();
px4_setup_pwm();
px4_setup_safety();
px4_setup_uart();
px4_setup_sbus();
px4_setup_drivers();
px4_setup_canbus();
}
#endif // HAL_BOARD_PX4