aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/drivers/px4io/px4io.cpp10
-rw-r--r--src/modules/px4iofirmware/controls.c49
-rw-r--r--src/modules/px4iofirmware/protocol.h14
-rw-r--r--src/modules/px4iofirmware/px4io.h3
-rw-r--r--src/modules/px4iofirmware/registers.c8
-rw-r--r--src/modules/px4iofirmware/sbus.c21
6 files changed, 76 insertions, 29 deletions
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 4bdf530a7..382acb6d0 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1763,6 +1763,7 @@ PX4IO::print_status()
printf("%u bytes free\n",
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
+ uint16_t io_status_flags = flags;
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
flags,
((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
@@ -1770,8 +1771,6 @@ PX4IO::print_status()
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""),
- (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""),
- (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""),
((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""),
@@ -1824,6 +1823,11 @@ PX4IO::print_status()
printf("\n");
uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
printf("%d raw R/C inputs", raw_inputs);
+ flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS);
+ printf("status 0x%04x%s", flags,
+ (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""),
+ (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : "")
+ );
for (unsigned i = 0; i < raw_inputs; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i));
@@ -1831,7 +1835,7 @@ PX4IO::print_status()
printf("\n");
if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) {
- int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA);
+ int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA);
printf("RC data (PPM frame len) %u us\n", frame_len);
if ((frame_len - raw_inputs * 2000 - 3000) < 0) {
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index ed37cc43d..1cac977d7 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -97,28 +97,57 @@ controls_tick() {
/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
uint16_t rssi = 0;
+#ifdef ADC_RSSI
+ unsigned counts = adc_measure(ADC_RSSI);
+ if (counts != 0xffff) {
+ /* use 1:1 scaling on 3.3V ADC input */
+ unsigned mV = counts * 3300 / 4096;
+
+ /* scale to 0..253 */
+ rssi = mV / 13;
+ }
+#endif
+
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
if (dsm_updated) {
- r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+ r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
- r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
+ r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
else
- r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
+ r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
- rssi = 255;
}
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
- bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
+ bool sbus_failsafe, sbus_frame_drop;
+ bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
+
if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
+
+ rssi = 255;
+
+ if (sbus_frame_drop) {
+ r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
+ rssi = 100;
+ } else {
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
+ }
+
+ if (sbus_failsafe) {
+ r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+ rssi = 0;
+ } else {
+ r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+ }
+
}
/* switch S.Bus output pin as needed */
@@ -136,12 +165,9 @@ controls_tick() {
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
- bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]);
+ bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
if (ppm_updated) {
- /* XXX sample RSSI properly here */
- rssi = 255;
-
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
}
perf_end(c_gather_ppm);
@@ -150,6 +176,9 @@ controls_tick() {
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
+ /* store RSSI */
+ r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
+
/*
* In some cases we may have received a frame, but input has still
* been lost.
@@ -247,7 +276,7 @@ controls_tick() {
* This might happen if a protocol-based receiver returns an update
* that contains no channels that we have mapped.
*/
- if (assigned_channels == 0 || rssi == 0) {
+ if (assigned_channels == 0 || (r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_FAILSAFE))) {
rc_input_lost = true;
} else {
/* set RC OK flag */
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index e5bef6eb3..738dc7d6e 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -111,7 +111,6 @@
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
-#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */
#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) /* [1] VBatt is very close to regulator dropout */
@@ -128,8 +127,6 @@
#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 */
-#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
-#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -140,7 +137,16 @@
/* array of raw RC input values, microseconds */
#define PX4IO_PAGE_RAW_RC_INPUT 4
#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */
-#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */
+#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */
+#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */
+#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */
+#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */
+
+#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
+#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
+#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */
+#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */
+#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */
/* array of scaled RC input values, -10000..10000 */
#define PX4IO_PAGE_RC_INPUT 5
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index ee9d5b0c4..a2e1ed6cd 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -96,6 +96,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
+#define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID]
#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE])
@@ -215,7 +216,7 @@ extern int dsm_init(const char *device);
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
extern void dsm_bind(uint16_t cmd, int pulses);
extern int sbus_init(const char *device);
-extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
+extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
/** global debug level for isr_debug() */
extern volatile uint8_t debug_level;
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 2c437d2c0..477efa082 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -90,8 +90,6 @@ uint16_t r_page_status[] = {
[PX4IO_P_STATUS_VSERVO] = 0,
[PX4IO_P_STATUS_VRSSI] = 0,
[PX4IO_P_STATUS_PRSSI] = 0,
- [PX4IO_P_STATUS_NRSSI] = 0,
- [PX4IO_P_STATUS_RC_DATA] = 0
};
/**
@@ -116,6 +114,12 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT];
uint16_t r_page_raw_rc_input[] =
{
[PX4IO_P_RAW_RC_COUNT] = 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
};
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 495447740..32ab2a698 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -87,7 +87,7 @@ static unsigned partial_frame_count;
unsigned sbus_frame_drops;
-static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
int
sbus_init(const char *device)
@@ -118,7 +118,7 @@ sbus_init(const char *device)
}
bool
-sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels)
+sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
{
ssize_t ret;
hrt_abstime now;
@@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_
* decode it.
*/
partial_frame_count = 0;
- return sbus_decode(now, values, num_values, rssi, max_channels);
+ return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels);
}
/*
@@ -215,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
};
static bool
-sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values)
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f)) {
@@ -289,20 +289,23 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
/* decode and handle failsafe and frame-lost flags */
if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
/* report that we failed to read anything valid off the receiver */
- *rssi = 0;
+ *sbus_failsafe = true;
+ *sbus_frame_drop = true;
return false;
}
else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
- /* set a special warning flag or try to calculate some kind of RSSI information - to be implemented
+ /* set a special warning flag
*
* Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
* condition as fail-safe greatly reduces the reliability and range of the radio link,
* e.g. by prematurely issueing return-to-launch!!! */
- *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
+ *sbus_failsafe = false;
+ *sbus_frame_drop = true;
+ } else {
+ *sbus_failsafe = false;
+ *sbus_frame_drop = false;
}
- *rssi = 255;
-
return true;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-27 15:41:28 +0000