void AP_BoardConfig::init()
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
/* configurre the FMU driver for the right number of PWMs */
// ensure only valid values are set, rounding up
if (_pwm_count > 6) _pwm_count.set(6);
if (_pwm_count < 0) _pwm_count.set(0);
if (_pwm_count == 1) _pwm_count.set(2);
if (_pwm_count == 3) _pwm_count.set(4);
if (_pwm_count == 5) _pwm_count.set(6);
int fd = open("/dev/px4fmu", 0);
if (fd == -1) {
AP_HAL::panic("Unable to open /dev/px4fmu");
}
if (ioctl(fd, PWM_SERVO_SET_COUNT, _pwm_count.get()) != 0) {
hal.console->printf("RCOutput: Unable to setup alt PWM to %u channels\n", _pwm_count.get());
}
close(fd);
hal.uartC->set_flow_control((AP_HAL::UARTDriver::flow_control)_ser1_rtscts.get());
if (hal.uartD != NULL) {
hal.uartD->set_flow_control((AP_HAL::UARTDriver::flow_control)_ser2_rtscts.get());
}
if (_safety_enable.get() == 0) {
hal.rcout->force_safety_off();
}
if (_sbus_out_enable.get() == 1) {
fd = open("/dev/px4io", 0);
if (fd == -1 || ioctl(fd, SBUS_SET_PROTO_VERSION, 1) != 0) {
hal.console->printf("SBUS: Unable to setup SBUS output\n");
}
if (fd != -1) {
close(fd);
}
}
#if !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
if (_can_enable == 1) {
const char *args[] = { "uavcan", "start", NULL };
int ret = uavcan_main(3, args);
if (ret != 0) {
hal.console->printf("UAVCAN: failed to start\n");
} else {
hal.console->printf("UAVCAN: started\n");
// give some time for CAN bus initialisation
hal.scheduler->delay(1500);
}
}
#endif
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
/* configure the VRBRAIN driver for the right number of PWMs */
#endif
}
void AP_BoardConfig::init()
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
/* 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 },
};
uint8_t mode_parm = (uint8_t)_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 (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);
}
}
// 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)(0x0000FFFF & _ignore_safety_channels)) != 0) {
hal.console->printf("IGNORE_SAFETY failed\n");
}
close(px4io_fd);
}
hal.uartC->set_flow_control((AP_HAL::UARTDriver::flow_control)_ser1_rtscts.get());
if (hal.uartD != NULL) {
hal.uartD->set_flow_control((AP_HAL::UARTDriver::flow_control)_ser2_rtscts.get());
}
if (_safety_enable.get() == 0) {
hal.rcout->force_safety_off();
}
if (_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 (i=0; i<ARRAY_SIZE(rates); i++) {
if (rates[i].value == _sbus_out_rate) {
rate = rates[i].rate;
}
}
if (!hal.rcout->enable_sbus_out(rate)) {
hal.console->printf("Failed to enable SBUS out\n");
}
}
#if !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
if (_can_enable >= 1) {
const char *args[] = { "uavcan", "start", NULL };
int ret = uavcan_main(3, args);
if (ret != 0) {
hal.console->printf("UAVCAN: failed to start\n");
} else {
hal.console->printf("UAVCAN: started\n");
// give some time for CAN bus initialisation
hal.scheduler->delay(2000);
}
}
if (_can_enable >= 2) {
const char *args[] = { "uavcan", "start", "fw", NULL };
int ret = uavcan_main(4, args);
if (ret != 0) {
hal.console->printf("UAVCAN: failed to start servers\n");
} else {
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
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
/* configure the VRBRAIN driver for the right number of PWMs */
#endif
// let the HAL know the target temperature. We pass a pointer as
// we want the user to be able to change the parameter without
// rebooting
hal.util->set_imu_target_temp((int8_t *)&_imu_target_temperature);
}
void AP_BoardConfig::init()
{
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
/* configurre the FMU driver for the right number of PWMs */
// ensure only valid values are set, rounding up
if (_pwm_count > 6) _pwm_count.set(6);
if (_pwm_count < 0) _pwm_count.set(0);
if (_pwm_count == 1) _pwm_count.set(2);
if (_pwm_count == 3) _pwm_count.set(4);
if (_pwm_count == 5) _pwm_count.set(6);
int fd = open("/dev/px4fmu", 0);
if (fd == -1) {
AP_HAL::panic("Unable to open /dev/px4fmu");
}
if (ioctl(fd, PWM_SERVO_SET_COUNT, _pwm_count.get()) != 0) {
hal.console->printf("RCOutput: Unable to setup alt PWM to %u channels\n", _pwm_count.get());
}
close(fd);
hal.uartC->set_flow_control((AP_HAL::UARTDriver::flow_control)_ser1_rtscts.get());
if (hal.uartD != NULL) {
hal.uartD->set_flow_control((AP_HAL::UARTDriver::flow_control)_ser2_rtscts.get());
}
if (_safety_enable.get() == 0) {
hal.rcout->force_safety_off();
}
if (_sbus_out_rate.get() >= 1) {
fd = open("/dev/px4io", 0);
if (fd == -1) {
hal.console->printf("SBUS: Unable to open px4io for sbus\n");
} else {
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 == _sbus_out_rate) {
rate = rates[i].rate;
}
}
for (uint8_t tries=0; tries<10; tries++) {
if (ioctl(fd, SBUS_SET_PROTO_VERSION, 1) != 0) {
continue;
}
if (ioctl(fd, PWM_SERVO_SET_SBUS_RATE, rate) != 0) {
continue;
}
break;
}
close(fd);
}
}
#if !defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
if (_can_enable >= 1) {
const char *args[] = { "uavcan", "start", NULL };
int ret = uavcan_main(3, args);
if (ret != 0) {
hal.console->printf("UAVCAN: failed to start\n");
} else {
hal.console->printf("UAVCAN: started\n");
// give some time for CAN bus initialisation
hal.scheduler->delay(2000);
}
}
if (_can_enable >= 2) {
const char *args[] = { "uavcan", "start", "fw", NULL };
int ret = uavcan_main(4, args);
if (ret != 0) {
hal.console->printf("UAVCAN: failed to start servers\n");
} else {
uint32_t start_wait_ms = AP_HAL::millis();
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
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
/* configure the VRBRAIN driver for the right number of PWMs */
#endif
}
