aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--apps/drivers/px4io/px4io.cpp30
-rw-r--r--apps/px4io/comms.c13
-rw-r--r--apps/px4io/mixer.cpp34
-rw-r--r--apps/px4io/protocol.h8
-rw-r--r--apps/px4io/px4io.h14
5 files changed, 84 insertions, 15 deletions
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index f24beaf53..07c65d8cd 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -72,6 +72,7 @@
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/scheduling_priorities.h>
+#include <systemlib/param/param.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
@@ -608,6 +609,35 @@ PX4IO::io_send()
/* set desired PWM output rate */
cmd.servo_rate = _update_rate;
+ /* maintaing the standard order of Roll, Pitch, Yaw, Throttle */
+ cmd.rc_map[0] = param_find("RC_MAP_ROLL");
+ cmd.rc_map[1] = param_find("RC_MAP_PITCH");
+ cmd.rc_map[2] = param_find("RC_MAP_YAW");
+ cmd.rc_map[3] = param_find("RC_MAP_THROTTLE");
+
+ /* set the individual channel properties */
+ char nbuf[16];
+ for (unsigned i = 0; i < 4; i++) {
+ sprintf(nbuf, "RC%d_MIN", i);
+ cmd.rc_min[i] = (uint16_t)param_find(nbuf);
+ }
+ for (unsigned i = 0; i < 4; i++) {
+ sprintf(nbuf, "RC%d_TRIM", i);
+ cmd.rc_trim[i] = (uint16_t)param_find(nbuf);
+ }
+ for (unsigned i = 0; i < 4; i++) {
+ sprintf(nbuf, "RC%d_MAX", i);
+ cmd.rc_max[i] = (uint16_t)param_find(nbuf);
+ }
+ for (unsigned i = 0; i < 4; i++) {
+ sprintf(nbuf, "RC%d_REV", i);
+ cmd.rc_rev[i] = (uint16_t)param_find(nbuf);
+ }
+ for (unsigned i = 0; i < 4; i++) {
+ sprintf(nbuf, "RC%d_DZ", i);
+ cmd.rc_dz[i] = (uint16_t)param_find(nbuf);
+ }
+
ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
if (ret)
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index c208db3ad..2733877a2 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -207,6 +207,19 @@ comms_handle_command(const void *buffer, size_t length)
system_state.servo_rate = new_servo_rate;
}
+ /* fetch the rc mappings */
+ for (unsigned i = 0; i < 4; i++)
+ system_state.rc_map[i] = cmd->rc_map[i];
+
+ /* fetch the rc channel attributes */
+ for (unsigned i = 0; i < 4; i++) {
+ system_state.rc_min[i] = cmd->rc_min[i];
+ system_state.rc_trim[i] = cmd->rc_trim[i];
+ system_state.rc_max[i] = cmd->rc_max[i];
+ system_state.rc_rev[i] = cmd->rc_rev[i];
+ system_state.rc_dz[i] = cmd->rc_dz[i];
+ }
+
/*
* update servo values immediately.
* the updates are done in addition also
diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index 98232a1a2..d65602587 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -67,6 +67,12 @@ extern "C" {
*/
#define FMU_INPUT_DROP_LIMIT_US 200000
+/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
+#define ROLL 0
+#define PITCH 1
+#define YAW 2
+#define THROTTLE 3
+
/* current servo arm/disarm state */
bool mixer_servos_armed = false;
@@ -112,22 +118,22 @@ mixer_tick(void)
/* when override is on or the fmu is not available, but RC is present */
control_count = system_state.rc_channels;
- // Remap the channels based on the radio type.
- // FMU: Roll, Pitch, Yaw, Throttle
- // Graupner: Throttle, Roll, Pitch, Yaw
sched_lock();
- rc_channel_data[0] = system_state.rc_channel_data[1];
- rc_channel_data[1] = system_state.rc_channel_data[2];
- rc_channel_data[2] = system_state.rc_channel_data[3];
- rc_channel_data[3] = system_state.rc_channel_data[0];
- // AETR
- //rc_channel_data[0] = system_state.rc_channel_data[0];
- //rc_channel_data[1] = system_state.rc_channel_data[1];
- //rc_channel_data[2] = system_state.rc_channel_data[3];
- //rc_channel_data[3] = system_state.rc_channel_data[2];
- for (unsigned i = 4; i < system_state.rc_channels; i++) {
+
+ /* remap roll, pitch, yaw and throttle from RC specific to internal ordering */
+ rc_channel_data[ROLL] = system_state.rc_channel_data[system_state.rc_map[ROLL]];
+ rc_channel_data[PITCH] = system_state.rc_channel_data[system_state.rc_map[PITCH]];
+ rc_channel_data[YAW] = system_state.rc_channel_data[system_state.rc_map[YAW]];
+ rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE]];
+
+ /* get the remaining channels, no remapping needed */
+ for (unsigned i = 4; i < system_state.rc_channels; i++)
rc_channel_data[i] = system_state.rc_channel_data[i];
- }
+
+ /* scale the control inputs */
+ rc_channel_data[THROTTLE] = ((rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) /
+ (system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000 + 1000;
+
control_values = &rc_channel_data[0];
sched_unlock();
} else {
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 3a049bb29..2632604ca 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -59,7 +59,13 @@ struct px4io_command {
bool relay_state[PX4IO_RELAY_CHANNELS]; /**< relay states as requested by FMU */
bool arm_ok; /**< FMU allows full arming */
bool vector_flight_mode_ok; /**< FMU aquired a valid position lock, ready for pos control */
- bool manual_override_ok; /**< if true, IO performs a direct manual override */
+ bool manual_override_ok; /**< if true, IO performs a direct manual override */
+ uint16_t rc_map[4]; /**< channel ordering of roll, pitch, yaw, throttle */
+ uint16_t rc_min[4]; /**< min value for each channel */
+ uint16_t rc_trim[4]; /**< trim value for each channel */
+ uint16_t rc_max[4]; /**< max value for each channel */
+ uint16_t rc_rev[4]; /**< rev value for each channel */
+ uint16_t rc_dz[4]; /**< dz value for each channel */
};
/**
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index fef0a9ba4..ef1584026 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -73,6 +73,20 @@ struct sys_state_s {
uint16_t servo_rate; /* output rate of servos in Hz */
/**
+ * Remote control input(s) channel mappings
+ */
+ uint16_t rc_map[4];
+
+ /**
+ * Remote control channel attributes
+ */
+ uint16_t rc_min[4];
+ uint16_t rc_trim[4];
+ uint16_t rc_max[4];
+ uint16_t rc_rev[4];
+ uint16_t rc_dz[4];
+
+ /**
* Data from the remote control input(s)
*/
unsigned rc_channels;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-08 15:15:14 +0000