forked from Archive/PX4-Autopilot
Tweak protocol register assignments and add new registers to accommodate differences in IOv2.
This commit is contained in:
px4dev
parent
a65a1237f0
commit
8fa226c909
|
|
@ -134,6 +134,7 @@ private:
|
|||
PX4IO_interface *_interface;
|
||||
|
||||
// XXX
|
||||
unsigned _hardware;
|
||||
unsigned _max_actuators;
|
||||
unsigned _max_controls;
|
||||
unsigned _max_rc_input;
|
||||
|
|
@ -318,6 +319,7 @@ PX4IO *g_dev;
|
|||
PX4IO::PX4IO(PX4IO_interface *interface) :
|
||||
CDev("px4io", "/dev/px4io"),
|
||||
_interface(interface),
|
||||
_hardware(0),
|
||||
_max_actuators(0),
|
||||
_max_controls(0),
|
||||
_max_rc_input(0),
|
||||
|
|
@ -387,18 +389,25 @@ PX4IO::init()
|
|||
return ret;
|
||||
|
||||
/* get some parameters */
|
||||
unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
|
||||
if (protocol != PX4IO_PROTOCOL_VERSION) {
|
||||
log("protocol/firmware mismatch");
|
||||
mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
|
||||
return -1;
|
||||
}
|
||||
_hardware = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION);
|
||||
_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
|
||||
_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
|
||||
_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
|
||||
_max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT);
|
||||
_max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
|
||||
_max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
|
||||
if ((_max_actuators < 1) || (_max_actuators > 255) ||
|
||||
(_max_relays < 1) || (_max_relays > 255) ||
|
||||
(_max_relays > 32) ||
|
||||
(_max_transfer < 16) || (_max_transfer > 255) ||
|
||||
(_max_rc_input < 1) || (_max_rc_input > 255)) {
|
||||
|
||||
log("failed getting parameters from PX4IO");
|
||||
mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort.");
|
||||
log("config read error");
|
||||
mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort.");
|
||||
return -1;
|
||||
}
|
||||
if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
|
||||
|
|
@ -1122,7 +1131,7 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned
|
|||
|
||||
int ret = _interface->set_reg(page, offset, values, num_values);
|
||||
if (ret != OK)
|
||||
debug("io_reg_set: error %d", ret);
|
||||
debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, errno);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
@ -1143,7 +1152,7 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v
|
|||
|
||||
int ret = _interface->get_reg(page, offset, values, num_values);
|
||||
if (ret != OK)
|
||||
debug("io_reg_get: data error %d", ret);
|
||||
debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, errno);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
@ -1237,9 +1246,9 @@ void
|
|||
PX4IO::print_status()
|
||||
{
|
||||
/* basic configuration */
|
||||
printf("protocol %u software %u bootloader %u buffer %uB\n",
|
||||
printf("protocol %u hardware %u bootloader %u buffer %uB\n",
|
||||
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
|
||||
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION),
|
||||
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION),
|
||||
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
|
||||
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
|
||||
printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
|
||||
|
|
@ -1268,7 +1277,7 @@ PX4IO::print_status()
|
|||
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : ""));
|
||||
uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
|
||||
printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
|
||||
printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n",
|
||||
alarms,
|
||||
((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""),
|
||||
((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""),
|
||||
|
|
@ -1276,18 +1285,26 @@ PX4IO::print_status()
|
|||
((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""),
|
||||
((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""),
|
||||
((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""),
|
||||
((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""));
|
||||
((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""),
|
||||
((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT) ? " VSERVO_FAULT" : ""));
|
||||
/* now clear alarms */
|
||||
io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF);
|
||||
|
||||
printf("vbatt %u ibatt %u vbatt scale %u\n",
|
||||
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
|
||||
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
|
||||
printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
|
||||
(double)_battery_amp_per_volt,
|
||||
(double)_battery_amp_bias,
|
||||
(double)_battery_mamphour_total);
|
||||
if (_hardware == 1) {
|
||||
printf("vbatt %u ibatt %u vbatt scale %u\n",
|
||||
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
|
||||
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
|
||||
printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
|
||||
(double)_battery_amp_per_volt,
|
||||
(double)_battery_amp_bias,
|
||||
(double)_battery_mamphour_total);
|
||||
} else if (_hardware == 2) {
|
||||
printf("vservo %u vservo scale %u\n",
|
||||
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO),
|
||||
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
|
||||
printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI));
|
||||
}
|
||||
printf("actuators");
|
||||
for (unsigned i = 0; i < _max_actuators; i++)
|
||||
printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
|
||||
|
|
|
|||
|
|
@ -66,7 +66,7 @@ controls_init(void)
|
|||
sbus_init("/dev/ttyS2");
|
||||
|
||||
/* default to a 1:1 input map, all enabled */
|
||||
for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
|
||||
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
|
||||
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
|
||||
|
||||
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
|
||||
|
|
@ -117,7 +117,7 @@ controls_tick() {
|
|||
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
|
||||
perf_end(c_gather_ppm);
|
||||
|
||||
ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
|
||||
ASSERT(r_raw_rc_count <= PX4IO_CONTROL_CHANNELS);
|
||||
|
||||
/*
|
||||
* In some cases we may have received a frame, but input has still
|
||||
|
|
@ -190,7 +190,7 @@ controls_tick() {
|
|||
|
||||
/* and update the scaled/mapped version */
|
||||
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
|
||||
ASSERT(mapped < MAX_CONTROL_CHANNELS);
|
||||
ASSERT(mapped < PX4IO_CONTROL_CHANNELS);
|
||||
|
||||
/* invert channel if pitch - pulling the lever down means pitching up by convention */
|
||||
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
|
||||
|
|
@ -202,7 +202,7 @@ controls_tick() {
|
|||
}
|
||||
|
||||
/* set un-assigned controls to zero */
|
||||
for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
|
||||
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
|
||||
if (!(assigned_channels & (1 << i)))
|
||||
r_rc_values[i] = 0;
|
||||
}
|
||||
|
|
@ -312,8 +312,8 @@ ppm_input(uint16_t *values, uint16_t *num_values)
|
|||
|
||||
/* PPM data exists, copy it */
|
||||
*num_values = ppm_decoded_channels;
|
||||
if (*num_values > MAX_CONTROL_CHANNELS)
|
||||
*num_values = MAX_CONTROL_CHANNELS;
|
||||
if (*num_values > PX4IO_CONTROL_CHANNELS)
|
||||
*num_values = PX4IO_CONTROL_CHANNELS;
|
||||
|
||||
for (unsigned i = 0; i < *num_values; i++)
|
||||
values[i] = ppm_buffer[i];
|
||||
|
|
|
|||
|
|
@ -154,7 +154,7 @@ mixer_tick(void)
|
|||
if (source == MIX_FAILSAFE) {
|
||||
|
||||
/* copy failsafe values to the servo outputs */
|
||||
for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
|
||||
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
|
||||
r_page_servos[i] = r_page_servo_failsafe[i];
|
||||
|
||||
/* safe actuators for FMU feedback */
|
||||
|
|
@ -164,11 +164,11 @@ mixer_tick(void)
|
|||
|
||||
} else if (source != MIX_NONE) {
|
||||
|
||||
float outputs[IO_SERVO_COUNT];
|
||||
float outputs[PX4IO_SERVO_COUNT];
|
||||
unsigned mixed;
|
||||
|
||||
/* mix */
|
||||
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
|
||||
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
|
||||
|
||||
/* scale to PWM and update the servo outputs as required */
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
|
|
@ -180,7 +180,7 @@ mixer_tick(void)
|
|||
r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
|
||||
|
||||
}
|
||||
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
|
||||
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
|
||||
r_page_servos[i] = 0;
|
||||
}
|
||||
|
||||
|
|
@ -215,7 +215,7 @@ mixer_tick(void)
|
|||
|
||||
if (mixer_servos_armed) {
|
||||
/* update the servo outputs. */
|
||||
for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
|
||||
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
|
||||
up_pwm_servo_set(i, r_page_servos[i]);
|
||||
}
|
||||
}
|
||||
|
|
@ -349,11 +349,11 @@ mixer_set_failsafe()
|
|||
return;
|
||||
|
||||
/* set failsafe defaults to the values for all inputs = 0 */
|
||||
float outputs[IO_SERVO_COUNT];
|
||||
float outputs[PX4IO_SERVO_COUNT];
|
||||
unsigned mixed;
|
||||
|
||||
/* mix */
|
||||
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
|
||||
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
|
||||
|
||||
/* scale to PWM and update the servo outputs as required */
|
||||
for (unsigned i = 0; i < mixed; i++) {
|
||||
|
|
@ -364,7 +364,7 @@ mixer_set_failsafe()
|
|||
}
|
||||
|
||||
/* disable the rest of the outputs */
|
||||
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
|
||||
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
|
||||
r_page_servo_failsafe[i] = 0;
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -62,12 +62,11 @@
|
|||
* Note that the implementation of readable pages prefers registers within
|
||||
* readable pages to be densely packed. Page numbers do not need to be
|
||||
* packed.
|
||||
*
|
||||
* Definitions marked 1 are only valid on PX4IOv1 boards. Likewise,
|
||||
* [2] denotes definitions specific to the PX4IOv2 board.
|
||||
*/
|
||||
|
||||
#define PX4IO_CONTROL_CHANNELS 8
|
||||
#define PX4IO_INPUT_CHANNELS 12
|
||||
#define PX4IO_RELAY_CHANNELS 4
|
||||
|
||||
/* Per C, this is safe for all 2's complement systems */
|
||||
#define REG_TO_SIGNED(_reg) ((int16_t)(_reg))
|
||||
#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed))
|
||||
|
|
@ -75,10 +74,12 @@
|
|||
#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f)
|
||||
#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
|
||||
|
||||
#define PX4IO_PROTOCOL_VERSION 2
|
||||
|
||||
/* static configuration page */
|
||||
#define PX4IO_PAGE_CONFIG 0
|
||||
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */
|
||||
#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */
|
||||
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */
|
||||
#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */
|
||||
#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */
|
||||
#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */
|
||||
#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */
|
||||
|
|
@ -107,16 +108,20 @@
|
|||
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
|
||||
|
||||
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
|
||||
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
|
||||
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */
|
||||
#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */
|
||||
#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */
|
||||
#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */
|
||||
#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */
|
||||
#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */
|
||||
#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */
|
||||
#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */
|
||||
#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */
|
||||
#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */
|
||||
|
||||
#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */
|
||||
#define PX4IO_P_STATUS_IBATT 5 /* battery current (raw ADC) */
|
||||
#define PX4IO_P_STATUS_VBATT 4 /* [1] battery voltage in mV */
|
||||
#define PX4IO_P_STATUS_IBATT 5 /* [1] battery current (raw ADC) */
|
||||
#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
|
||||
#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
|
||||
#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */
|
||||
|
||||
/* array of post-mix actuator outputs, -10000..10000 */
|
||||
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
|
@ -142,7 +147,7 @@
|
|||
#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
|
||||
|
||||
/* setup page */
|
||||
#define PX4IO_PAGE_SETUP 100
|
||||
#define PX4IO_PAGE_SETUP 50
|
||||
#define PX4IO_P_SETUP_FEATURES 0
|
||||
|
||||
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
|
||||
|
|
@ -155,18 +160,27 @@
|
|||
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
|
||||
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
|
||||
#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
|
||||
|
||||
#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
|
||||
#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */
|
||||
#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* [1] power relay 1 */
|
||||
#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* [1] power relay 2 */
|
||||
#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* [1] accessory power 1 */
|
||||
#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* [1] accessory power 2 */
|
||||
|
||||
#define PX4IO_P_SETUP_VBATT_SCALE 6 /* [1] battery voltage correction factor (float) */
|
||||
#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* [2] servo voltage correction factor (float) */
|
||||
/* 7 */
|
||||
/* 8 */
|
||||
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
|
||||
|
||||
/* autopilot control values, -10000..10000 */
|
||||
#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */
|
||||
#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */
|
||||
|
||||
/* raw text load to the mixer parser - ignores offset */
|
||||
#define PX4IO_PAGE_MIXERLOAD 102
|
||||
#define PX4IO_PAGE_MIXERLOAD 52
|
||||
|
||||
/* R/C channel config */
|
||||
#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */
|
||||
#define PX4IO_PAGE_RC_CONFIG 53 /* R/C input configuration */
|
||||
#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */
|
||||
#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */
|
||||
#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */
|
||||
|
|
@ -178,13 +192,13 @@
|
|||
#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */
|
||||
|
||||
/* PWM output - overrides mixer */
|
||||
#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
#define PX4IO_PAGE_DIRECT_PWM 54 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* PWM failsafe values - zero disables the output */
|
||||
#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
#define PX4IO_PAGE_FAILSAFE_PWM 55 /* 0..CONFIG_ACTUATOR_COUNT-1 */
|
||||
|
||||
/* Debug and test page - not used in normal operation */
|
||||
#define PX4IO_PAGE_TEST 127
|
||||
#define PX4IO_PAGE_TEST 127
|
||||
#define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -54,8 +54,9 @@
|
|||
/*
|
||||
* Constants and limits.
|
||||
*/
|
||||
#define MAX_CONTROL_CHANNELS 12
|
||||
#define IO_SERVO_COUNT 8
|
||||
#define PX4IO_SERVO_COUNT 8
|
||||
#define PX4IO_CONTROL_CHANNELS 8
|
||||
#define PX4IO_INPUT_CHANNELS 12
|
||||
|
||||
/*
|
||||
* Debug logging
|
||||
|
|
@ -124,34 +125,41 @@ extern struct sys_state_s system_state;
|
|||
/*
|
||||
* GPIO handling.
|
||||
*/
|
||||
#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s))
|
||||
#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s))
|
||||
#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
|
||||
#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s))
|
||||
#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s))
|
||||
#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s))
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4IOV2
|
||||
|
||||
# define PX4IO_RELAY_CHANNELS 0
|
||||
# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
|
||||
|
||||
# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
|
||||
|
||||
# define PX4IO_ADC_CHANNEL_COUNT 2
|
||||
# define ADC_VSERVO 4
|
||||
# define ADC_RSSI 5
|
||||
|
||||
#else /* CONFIG_ARCH_BOARD_PX4IOV1 */
|
||||
|
||||
# define PX4IO_RELAY_CHANNELS 4
|
||||
# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
|
||||
# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
|
||||
# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
|
||||
# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
|
||||
# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
|
||||
|
||||
# define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT))
|
||||
# define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT))
|
||||
|
||||
# define PX4IO_ADC_CHANNEL_COUNT 2
|
||||
# define ADC_VBATT 4
|
||||
# define ADC_IN5 5
|
||||
|
||||
#ifdef GPIO_SERVO_PWR_EN
|
||||
# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
|
||||
#endif
|
||||
#ifdef GPIO_ACC1_PWR_EN
|
||||
# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
|
||||
#endif
|
||||
#ifdef GPIO_ACC2_PWR_EN
|
||||
# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
|
||||
#endif
|
||||
#ifdef GPIO_RELAY1_EN
|
||||
# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
|
||||
#endif
|
||||
#ifdef GPIO_RELAY2_EN
|
||||
# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
|
||||
#endif
|
||||
|
||||
#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT))
|
||||
#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT))
|
||||
#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY)
|
||||
|
||||
#define ADC_VBATT 4
|
||||
#define ADC_IN5 5
|
||||
#define ADC_CHANNEL_COUNT 2
|
||||
|
||||
/*
|
||||
* Mixer
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -58,14 +58,18 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt
|
|||
* Static configuration parameters.
|
||||
*/
|
||||
static const uint16_t r_page_config[] = {
|
||||
[PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */
|
||||
[PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */
|
||||
[PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_PROTOCOL_VERSION,
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4IOV2
|
||||
[PX4IO_P_CONFIG_HARDWARE_VERSION] = 2,
|
||||
#else
|
||||
[PX4IO_P_CONFIG_HARDWARE_VERSION] = 1,
|
||||
#endif
|
||||
[PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */
|
||||
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */
|
||||
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
|
||||
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT,
|
||||
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS,
|
||||
[PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT,
|
||||
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT,
|
||||
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_CONTROL_CHANNELS,
|
||||
[PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT,
|
||||
[PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
|
||||
};
|
||||
|
||||
|
|
@ -80,7 +84,10 @@ uint16_t r_page_status[] = {
|
|||
[PX4IO_P_STATUS_FLAGS] = 0,
|
||||
[PX4IO_P_STATUS_ALARMS] = 0,
|
||||
[PX4IO_P_STATUS_VBATT] = 0,
|
||||
[PX4IO_P_STATUS_IBATT] = 0
|
||||
[PX4IO_P_STATUS_IBATT] = 0,
|
||||
[PX4IO_P_STATUS_VSERVO] = 0,
|
||||
[PX4IO_P_STATUS_VRSSI] = 0,
|
||||
[PX4IO_P_STATUS_PRSSI] = 0
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -88,14 +95,14 @@ uint16_t r_page_status[] = {
|
|||
*
|
||||
* Post-mixed actuator values.
|
||||
*/
|
||||
uint16_t r_page_actuators[IO_SERVO_COUNT];
|
||||
uint16_t r_page_actuators[PX4IO_SERVO_COUNT];
|
||||
|
||||
/**
|
||||
* PAGE 3
|
||||
*
|
||||
* Servo PWM values
|
||||
*/
|
||||
uint16_t r_page_servos[IO_SERVO_COUNT];
|
||||
uint16_t r_page_servos[PX4IO_SERVO_COUNT];
|
||||
|
||||
/**
|
||||
* PAGE 4
|
||||
|
|
@ -105,7 +112,7 @@ uint16_t r_page_servos[IO_SERVO_COUNT];
|
|||
uint16_t r_page_raw_rc_input[] =
|
||||
{
|
||||
[PX4IO_P_RAW_RC_COUNT] = 0,
|
||||
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
|
||||
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -115,7 +122,7 @@ uint16_t r_page_raw_rc_input[] =
|
|||
*/
|
||||
uint16_t r_page_rc_input[] = {
|
||||
[PX4IO_P_RC_VALID] = 0,
|
||||
[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
|
||||
[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -148,7 +155,7 @@ volatile uint16_t r_page_setup[] =
|
|||
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
|
||||
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
|
||||
PX4IO_P_SETUP_ARMING_IO_ARM_OK)
|
||||
#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
|
||||
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
|
||||
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
|
||||
|
||||
/**
|
||||
|
|
@ -167,7 +174,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
|
|||
*
|
||||
* R/C channel input configuration.
|
||||
*/
|
||||
uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
|
||||
/* valid options */
|
||||
#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
|
||||
|
|
@ -183,7 +190,7 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
|
|||
*
|
||||
* Disable pulses as default.
|
||||
*/
|
||||
uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 };
|
||||
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
|
||||
|
||||
int
|
||||
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
|
||||
|
|
@ -234,7 +241,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
|
|||
case PX4IO_PAGE_FAILSAFE_PWM:
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
|
||||
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
|
||||
|
||||
/* XXX range-check value? */
|
||||
r_page_servo_failsafe[offset] = *values;
|
||||
|
|
@ -366,6 +373,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
#endif
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_VBATT_SCALE:
|
||||
r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value;
|
||||
break;
|
||||
|
||||
case PX4IO_P_SETUP_SET_DEBUG:
|
||||
r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value;
|
||||
isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
|
||||
|
|
@ -388,7 +399,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
|
||||
uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
|
||||
if (channel >= MAX_CONTROL_CHANNELS)
|
||||
if (channel >= PX4IO_CONTROL_CHANNELS)
|
||||
return -1;
|
||||
|
||||
/* disable the channel until we have a chance to sanity-check it */
|
||||
|
|
@ -433,7 +444,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
|
||||
count++;
|
||||
}
|
||||
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
|
||||
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) {
|
||||
count++;
|
||||
}
|
||||
|
||||
|
|
@ -497,6 +508,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
|
||||
/* PX4IO_P_STATUS_ALARMS maintained externally */
|
||||
|
||||
#ifdef ADC_VBATT
|
||||
/* PX4IO_P_STATUS_VBATT */
|
||||
{
|
||||
/*
|
||||
|
|
@ -530,7 +542,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
#ifdef ADC_IBATT
|
||||
/* PX4IO_P_STATUS_IBATT */
|
||||
{
|
||||
/*
|
||||
|
|
@ -540,26 +553,74 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
|||
FMU sort it out, with user selectable
|
||||
configuration for their sensor
|
||||
*/
|
||||
unsigned counts = adc_measure(ADC_IN5);
|
||||
unsigned counts = adc_measure(ADC_IBATT);
|
||||
if (counts != 0xffff) {
|
||||
r_page_status[PX4IO_P_STATUS_IBATT] = counts;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#ifdef ADC_VSERVO
|
||||
/* PX4IO_P_STATUS_VSERVO */
|
||||
{
|
||||
/*
|
||||
* Coefficients here derived by measurement of the 5-16V
|
||||
* range on one unit:
|
||||
*
|
||||
* V counts
|
||||
* 5 1001
|
||||
* 6 1219
|
||||
* 7 1436
|
||||
* 8 1653
|
||||
* 9 1870
|
||||
* 10 2086
|
||||
* 11 2303
|
||||
* 12 2522
|
||||
* 13 2738
|
||||
* 14 2956
|
||||
* 15 3172
|
||||
* 16 3389
|
||||
*
|
||||
* slope = 0.0046067
|
||||
* intercept = 0.3863
|
||||
*
|
||||
* Intercept corrected for best results @ 12V.
|
||||
*/
|
||||
unsigned counts = adc_measure(ADC_VSERVO);
|
||||
if (counts != 0xffff) {
|
||||
unsigned mV = (4150 + (counts * 46)) / 10 - 200;
|
||||
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
|
||||
|
||||
r_page_status[PX4IO_P_STATUS_VSERVO] = corrected;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
/* XXX PX4IO_P_STATUS_VRSSI */
|
||||
/* XXX PX4IO_P_STATUS_PRSSI */
|
||||
|
||||
SELECT_PAGE(r_page_status);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_RAW_ADC_INPUT:
|
||||
memset(r_page_scratch, 0, sizeof(r_page_scratch));
|
||||
#ifdef ADC_VBATT
|
||||
r_page_scratch[0] = adc_measure(ADC_VBATT);
|
||||
r_page_scratch[1] = adc_measure(ADC_IN5);
|
||||
#endif
|
||||
#ifdef ADC_IBATT
|
||||
r_page_scratch[1] = adc_measure(ADC_IBATT);
|
||||
#endif
|
||||
|
||||
#ifdef ADC_VSERVO
|
||||
r_page_scratch[0] = adc_measure(ADC_VSERVO);
|
||||
#endif
|
||||
#ifdef ADC_RSSI
|
||||
r_page_scratch[1] = adc_measure(ADC_RSSI);
|
||||
#endif
|
||||
SELECT_PAGE(r_page_scratch);
|
||||
break;
|
||||
|
||||
case PX4IO_PAGE_PWM_INFO:
|
||||
memset(r_page_scratch, 0, sizeof(r_page_scratch));
|
||||
for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
|
||||
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
|
||||
r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
|
||||
|
||||
SELECT_PAGE(r_page_scratch);
|
||||
|
|
@ -631,7 +692,7 @@ static void
|
|||
pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
|
||||
{
|
||||
for (unsigned pass = 0; pass < 2; pass++) {
|
||||
for (unsigned group = 0; group < IO_SERVO_COUNT; group++) {
|
||||
for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) {
|
||||
|
||||
/* get the channel mask for this rate group */
|
||||
uint32_t mask = up_pwm_servo_get_rate_group(group);
|
||||
|
|
|
|||
|
|
@ -210,7 +210,7 @@ int preflight_check_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
/* XXX needs inspection of all the _MAP params */
|
||||
// if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
|
||||
// if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) {
|
||||
// mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
|
||||
// /* give system time to flush error message in case there are more */
|
||||
// usleep(100000);
|
||||
|
|
|
|||
Loading…
Reference in New Issue