aboutsummaryrefslogtreecommitdiff
path: root/src/modules/px4iofirmware/registers.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/px4iofirmware/registers.c')
-rw-r--r--src/modules/px4iofirmware/registers.c365
1 files changed, 321 insertions, 44 deletions
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 9698a0f2f..97d25bbfa 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -41,8 +41,12 @@
#include <stdbool.h>
#include <stdlib.h>
+#include <string.h>
#include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
+#include <systemlib/systemlib.h>
+#include <stm32_pwr.h>
#include "px4io.h"
#include "protocol.h"
@@ -56,14 +60,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_PX4IO_V2
+ [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_RC_INPUT_CHANNELS,
+ [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT,
[PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
};
@@ -78,7 +86,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,
};
/**
@@ -86,14 +97,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
@@ -103,7 +114,13 @@ 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_FLAGS] = 0,
+ [PX4IO_P_RAW_RC_NRSSI] = 0,
+ [PX4IO_P_RAW_RC_DATA] = 0,
+ [PX4IO_P_RAW_FRAME_COUNT] = 0,
+ [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0,
+ [PX4IO_P_RAW_RC_DATA] = 0,
+ [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
};
/**
@@ -113,7 +130,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_RC_MAPPED_CONTROL_CHANNELS)] = 0
};
/**
@@ -131,22 +148,46 @@ uint16_t r_page_scratch[32];
*/
volatile uint16_t r_page_setup[] =
{
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+ /* default to RSSI ADC functionality */
+ [PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI,
+#else
[PX4IO_P_SETUP_FEATURES] = 0,
+#endif
[PX4IO_P_SETUP_ARMING] = 0,
[PX4IO_P_SETUP_PWM_RATES] = 0,
[PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
[PX4IO_P_SETUP_PWM_ALTRATE] = 200,
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
[PX4IO_P_SETUP_RELAYS] = 0,
+#endif
+#ifdef ADC_VSERVO
+ [PX4IO_P_SETUP_VSERVO_SCALE] = 10000,
+#else
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
+#endif
[PX4IO_P_SETUP_SET_DEBUG] = 0,
+ [PX4IO_P_SETUP_REBOOT_BL] = 0,
+ [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
};
-#define PX4IO_P_SETUP_FEATURES_VALID (0)
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
+ PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
+ PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
+ PX4IO_P_SETUP_FEATURES_PWM_RSSI)
+#else
+#define PX4IO_P_SETUP_FEATURES_VALID 0
+#endif
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
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)
+ PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
+ PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
+ PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
+ PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
+ PX4IO_P_SETUP_ARMING_LOCKDOWN)
+#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
/**
@@ -154,7 +195,7 @@ volatile uint16_t r_page_setup[] =
*
* Control values from the FMU.
*/
-volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
+volatile uint16_t r_page_controls[PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS];
/*
* PAGE 102 does not have a buffer.
@@ -165,7 +206,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_RC_INPUT_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)
@@ -178,10 +219,36 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
* PAGE 105
*
* Failsafe servo PWM values
+ *
+ * Disable pulses as default.
*/
-uint16_t r_page_servo_failsafe[IO_SERVO_COUNT];
+uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
-void
+/**
+ * PAGE 106
+ *
+ * minimum PWM values when armed
+ *
+ */
+uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN };
+
+/**
+ * PAGE 107
+ *
+ * maximum PWM values when armed
+ *
+ */
+uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX };
+
+/**
+ * PAGE 108
+ *
+ * disarmed PWM values for difficult ESCs
+ *
+ */
+uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+
+int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
@@ -191,7 +258,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
case PX4IO_PAGE_CONTROLS:
/* copy channel data */
- while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+ while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
r_page_controls[offset] = *values;
@@ -230,20 +297,112 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
case PX4IO_PAGE_FAILSAFE_PWM:
/* copy channel data */
- while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+ if (*values == 0) {
+ /* ignore 0 */
+ } else if (*values < PWM_LOWEST_MIN) {
+ r_page_servo_failsafe[offset] = PWM_LOWEST_MIN;
+ } else if (*values > PWM_HIGHEST_MAX) {
+ r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX;
+ } else {
+ r_page_servo_failsafe[offset] = *values;
+ }
- /* XXX range-check value? */
- r_page_servo_failsafe[offset] = *values;
+ /* flag the failsafe values as custom */
+ r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
+
+ offset++;
+ num_values--;
+ values++;
+ }
+ break;
+
+ case PX4IO_PAGE_CONTROL_MIN_PWM:
+
+ /* copy channel data */
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+ if (*values == 0) {
+ /* ignore 0 */
+ } else if (*values > PWM_HIGHEST_MIN) {
+ r_page_servo_control_min[offset] = PWM_HIGHEST_MIN;
+ } else if (*values < PWM_LOWEST_MIN) {
+ r_page_servo_control_min[offset] = PWM_LOWEST_MIN;
+ } else {
+ r_page_servo_control_min[offset] = *values;
+ }
offset++;
num_values--;
values++;
}
break;
+
+ case PX4IO_PAGE_CONTROL_MAX_PWM:
+
+ /* copy channel data */
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+ if (*values == 0) {
+ /* ignore 0 */
+ } else if (*values > PWM_HIGHEST_MAX) {
+ r_page_servo_control_max[offset] = PWM_HIGHEST_MAX;
+ } else if (*values < PWM_LOWEST_MAX) {
+ r_page_servo_control_max[offset] = PWM_LOWEST_MAX;
+ } else {
+ r_page_servo_control_max[offset] = *values;
+ }
+
+ offset++;
+ num_values--;
+ values++;
+ }
+ break;
+
+ case PX4IO_PAGE_DISARMED_PWM:
+ {
+ /* flag for all outputs */
+ bool all_disarmed_off = true;
+
+ /* copy channel data */
+ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+ if (*values == 0) {
+ /* 0 means disabling always PWM */
+ r_page_servo_disarmed[offset] = 0;
+ } else if (*values < PWM_LOWEST_MIN) {
+ r_page_servo_disarmed[offset] = PWM_LOWEST_MIN;
+ all_disarmed_off = false;
+ } else if (*values > PWM_HIGHEST_MAX) {
+ r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX;
+ all_disarmed_off = false;
+ } else {
+ r_page_servo_disarmed[offset] = *values;
+ all_disarmed_off = false;
+ }
+
+ offset++;
+ num_values--;
+ values++;
+ }
+
+ if (all_disarmed_off) {
+ /* disable PWM output if disarmed */
+ r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE);
+ } else {
+ /* enable PWM output always */
+ r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
+ }
+ }
+ break;
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
- mixer_handle_text(values, num_values * sizeof(*values));
+ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
+ return mixer_handle_text(values, num_values * sizeof(*values));
+ }
break;
default:
@@ -254,11 +413,13 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* iterate individual registers, set each in turn */
while (num_values--) {
if (registers_set_one(page, offset, *values))
- break;
+ return -1;
offset++;
values++;
}
+ break;
}
+ return 0;
}
static int
@@ -294,9 +455,35 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_P_SETUP_FEATURES:
value &= PX4IO_P_SETUP_FEATURES_VALID;
- r_setup_features = value;
- /* no implemented feature selection at this point */
+ /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */
+
+ /* switch S.Bus output pin as needed */
+ #ifdef ENABLE_SBUS_OUT
+ ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
+
+ /* disable the conflicting options */
+ if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
+ value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+ }
+ #endif
+
+ /* disable the conflicting options with ADC RSSI */
+ if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
+ value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+ PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
+ PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+ }
+
+ /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */
+ if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) {
+ value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+ PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
+ PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+ }
+
+ /* apply changes */
+ r_setup_features = value;
break;
@@ -311,8 +498,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* so that an in-air reset of FMU can not lead to a
* lockup of the IO arming state.
*/
- if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
- r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+
+ if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
}
r_setup_arming = value;
@@ -340,13 +528,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
break;
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
case PX4IO_P_SETUP_RELAYS:
value &= PX4IO_P_SETUP_RELAYS_VALID;
r_setup_relays = value;
- POWER_RELAY1(value & (1 << 0) ? 1 : 0);
- POWER_RELAY2(value & (1 << 1) ? 1 : 0);
- POWER_ACC1(value & (1 << 2) ? 1 : 0);
- POWER_ACC2(value & (1 << 3) ? 1 : 0);
+ POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
+ POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
+ POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
+ POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
+ break;
+#endif
+
+ case PX4IO_P_SETUP_VBATT_SCALE:
+ r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value;
break;
case PX4IO_P_SETUP_SET_DEBUG:
@@ -354,6 +548,27 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
break;
+ case PX4IO_P_SETUP_REBOOT_BL:
+ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
+ // don't allow reboot while armed
+ break;
+ }
+
+ // check the magic value
+ if (value != PX4IO_REBOOT_BL_MAGIC)
+ break;
+
+ // we schedule a reboot rather than rebooting
+ // immediately to allow the IO board to ACK
+ // the reboot command
+ schedule_reboot(100000);
+ break;
+
+ case PX4IO_P_SETUP_DSM:
+ dsm_bind(value & 0x0f, (value >> 4) & 0xF);
+ break;
+
default:
return -1;
}
@@ -361,9 +576,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_PAGE_RC_CONFIG: {
- /* do not allow a RC config change while fully armed */
- if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
- /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+ /**
+ * do not allow a RC config change while outputs armed
+ */
+ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
break;
}
@@ -371,7 +588,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_RC_INPUT_CHANNELS)
return -1;
/* disable the channel until we have a chance to sanity-check it */
@@ -391,6 +608,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
+ /* clear any existing RC disabled flag */
+ r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);
+
/* set all options except the enabled option */
conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
@@ -398,6 +618,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
/* this option is normally set last */
if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint8_t count = 0;
+ bool disabled = false;
/* assert min..center..max ordering */
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
@@ -416,7 +637,10 @@ 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] == UINT8_MAX) {
+ disabled = true;
+ } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
count++;
}
@@ -424,7 +648,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (count) {
isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
- } else {
+ } else if (!disabled) {
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
}
@@ -436,6 +660,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
/* case PX4IO_RC_PAGE_CONFIG */
}
+ case PX4IO_PAGE_TEST:
+ switch (offset) {
+ case PX4IO_P_TEST_LED:
+ LED_AMBER(value & 1);
+ break;
+ }
+ break;
+
default:
return -1;
}
@@ -472,6 +704,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 */
{
/*
@@ -505,7 +738,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 */
{
/*
@@ -515,26 +749,60 @@ 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 */
+ {
+ unsigned counts = adc_measure(ADC_VSERVO);
+ if (counts != 0xffff) {
+ // use 3:1 scaling on 3.3V ADC input
+ unsigned mV = counts * 9900 / 4096;
+ r_page_status[PX4IO_P_STATUS_VSERVO] = mV;
+ }
+ }
+#endif
+#ifdef ADC_RSSI
+ /* PX4IO_P_STATUS_VRSSI */
+ {
+ unsigned counts = adc_measure(ADC_RSSI);
+ if (counts != 0xffff) {
+ // use 1:1 scaling on 3.3V ADC input
+ unsigned mV = counts * 3300 / 4096;
+ r_page_status[PX4IO_P_STATUS_VRSSI] = mV;
+ }
+ }
+#endif
+ /* 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);
@@ -577,6 +845,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_FAILSAFE_PWM:
SELECT_PAGE(r_page_servo_failsafe);
break;
+ case PX4IO_PAGE_CONTROL_MIN_PWM:
+ SELECT_PAGE(r_page_servo_control_min);
+ break;
+ case PX4IO_PAGE_CONTROL_MAX_PWM:
+ SELECT_PAGE(r_page_servo_control_max);
+ break;
+ case PX4IO_PAGE_DISARMED_PWM:
+ SELECT_PAGE(r_page_servo_disarmed);
+ break;
default:
return -1;
@@ -606,7 +883,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);
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-11 13:15:52 +0000