From c21237667bc2802f675c74641d25f538db5f2c0c Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 4 Jul 2013 23:16:13 -0700 Subject: iov2 pin definition cleanup sweep --- src/modules/px4iofirmware/controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/px4iofirmware/controls.c') diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3cf9ca149..95103964e 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -59,10 +59,10 @@ static perf_counter_t c_gather_ppm; void controls_init(void) { - /* DSM input */ + /* DSM input (USART1) */ dsm_init("/dev/ttyS0"); - /* S.bus input */ + /* S.bus input (USART3) */ sbus_init("/dev/ttyS2"); /* default to a 1:1 input map, all enabled */ -- cgit v1.2.3 From 8fa226c909d5d34606e8f28bb0b54aeda8f91010 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Jul 2013 23:59:35 -0700 Subject: Tweak protocol register assignments and add new registers to accommodate differences in IOv2. --- src/drivers/px4io/px4io.cpp | 53 ++++++++---- src/modules/px4iofirmware/controls.c | 12 +-- src/modules/px4iofirmware/mixer.cpp | 16 ++-- src/modules/px4iofirmware/protocol.h | 52 +++++++----- src/modules/px4iofirmware/px4io.h | 58 +++++++------ src/modules/px4iofirmware/registers.c | 103 ++++++++++++++++++----- src/systemcmds/preflight_check/preflight_check.c | 2 +- 7 files changed, 198 insertions(+), 98 deletions(-) (limited to 'src/modules/px4iofirmware/controls.c') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 951bfe695..15e213afb 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -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)); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 95103964e..5a95a8aa9 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -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]; diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2193b526..d8c0e58ba 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -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; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 48ad4316f..fa57dfc3f 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -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 */ /** diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 0779ffd8f..b32782285 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -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 */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index b977375f4..f4541936b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -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); diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 4bcce18fb..6d1ecb321 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -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); -- cgit v1.2.3