forked from Archive/PX4-Autopilot
Quickly separated low-level raw RC from mapped / scaled RC, supports FMU PPM and IO PPM / Spektrum now
This commit is contained in:
Lorenz Meier
parent
39659e57f8
commit
e7f2c053c2
|
|
@ -57,15 +57,23 @@
|
|||
#define RC_INPUT_DEVICE_PATH "/dev/input_rc"
|
||||
|
||||
/**
|
||||
* Maximum number of R/C input channels in the system.
|
||||
* Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
|
||||
*/
|
||||
#define RC_INPUT_MAX_CHANNELS 16
|
||||
#define RC_INPUT_MAX_CHANNELS 18
|
||||
|
||||
/**
|
||||
* Input signal type, value is a control position from zero to 100
|
||||
* percent.
|
||||
*/
|
||||
typedef uint8_t rc_input_t;
|
||||
typedef uint16_t rc_input_t;
|
||||
|
||||
enum RC_INPUT_SOURCE {
|
||||
RC_INPUT_SOURCE_UNKNOWN = 0,
|
||||
RC_INPUT_SOURCE_PX4FMU_PPM,
|
||||
RC_INPUT_SOURCE_PX4IO_PPM,
|
||||
RC_INPUT_SOURCE_PX4IO_SPEKTRUM,
|
||||
RC_INPUT_SOURCE_PX4IO_SBUS
|
||||
};
|
||||
|
||||
/**
|
||||
* R/C input status structure.
|
||||
|
|
@ -74,10 +82,16 @@ typedef uint8_t rc_input_t;
|
|||
* on the board involved.
|
||||
*/
|
||||
struct rc_input_values {
|
||||
/** decoding time */
|
||||
uint64_t timestamp;
|
||||
|
||||
/** number of channels actually being seen */
|
||||
uint32_t channel_count;
|
||||
|
||||
/** desired pulse widths for each of the supported channels */
|
||||
/** Input source */
|
||||
enum RC_INPUT_SOURCE input_source;
|
||||
|
||||
/** measured pulse widths for each of the supported channels */
|
||||
rc_input_t values[RC_INPUT_MAX_CHANNELS];
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -61,9 +61,10 @@
|
|||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <systemlib/mixer/mixer.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_mixer.h>
|
||||
|
||||
#include <systemlib/mixer/mixer.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/hx_stream.h>
|
||||
#include <systemlib/err.h>
|
||||
|
|
@ -105,6 +106,9 @@ private:
|
|||
int _t_armed; ///< system armed control topic
|
||||
actuator_armed_s _armed; ///< system armed state
|
||||
|
||||
orb_advert_t _to_input_rc; ///< rc inputs from io
|
||||
rc_input_values _input_rc; ///< rc input values
|
||||
|
||||
orb_advert_t _t_outputs; ///< mixed outputs topic
|
||||
actuator_outputs_s _outputs; ///< mixed outputs
|
||||
|
||||
|
|
@ -317,9 +321,14 @@ PX4IO::task_main()
|
|||
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
|
||||
|
||||
/* advertise the mixed control outputs */
|
||||
memset(&_outputs, 0, sizeof(_outputs));
|
||||
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
|
||||
&_outputs);
|
||||
|
||||
/* advertise the rc inputs */
|
||||
memset(&_input_rc, 0, sizeof(_input_rc));
|
||||
_to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc);
|
||||
|
||||
/* poll descriptor */
|
||||
pollfd fds[3];
|
||||
fds[0].fd = _serial_fd;
|
||||
|
|
@ -456,7 +465,14 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
|
|||
}
|
||||
_connected = true;
|
||||
|
||||
/* XXX handle R/C inputs here ... needs code sharing/library */
|
||||
/* publish raw rc channel values from IO */
|
||||
_input_rc.timestamp = hrt_absolute_time();
|
||||
for (int i = 0; i < rep->channel_count; i++)
|
||||
{
|
||||
_input_rc.values[i] = rep->rc_channel[i];
|
||||
}
|
||||
|
||||
orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
|
||||
|
||||
/* remember the latched arming switch state */
|
||||
_switch_armed = rep->armed;
|
||||
|
|
|
|||
|
|
@ -338,7 +338,7 @@ static void hrt_call_invoke(void);
|
|||
/* decoded PPM buffer */
|
||||
#define PPM_MAX_CHANNELS 12
|
||||
__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
|
||||
__EXPORT unsigned ppm_decoded_channels;
|
||||
__EXPORT unsigned ppm_decoded_channels = 0;
|
||||
__EXPORT uint64_t ppm_last_valid_decode = 0;
|
||||
|
||||
/* PPM edge history */
|
||||
|
|
|
|||
|
|
@ -397,10 +397,6 @@ handle_message(mavlink_message_t *msg)
|
|||
|
||||
rc_hil.timestamp = hrt_absolute_time();
|
||||
rc_hil.chan_count = 4;
|
||||
rc_hil.chan[0].raw = 1500 + man.x / 2;
|
||||
rc_hil.chan[1].raw = 1500 + man.y / 2;
|
||||
rc_hil.chan[2].raw = 1500 + man.r / 2;
|
||||
rc_hil.chan[3].raw = 1500 + man.z / 2;
|
||||
|
||||
rc_hil.chan[0].scaled = man.x / 1000.0f;
|
||||
rc_hil.chan[1].scaled = man.y / 1000.0f;
|
||||
|
|
|
|||
|
|
@ -67,6 +67,7 @@ struct vehicle_global_position_s global_pos;
|
|||
struct vehicle_local_position_s local_pos;
|
||||
struct vehicle_status_s v_status;
|
||||
struct rc_channels_s rc;
|
||||
struct rc_input_values rc_raw;
|
||||
struct actuator_armed_s armed;
|
||||
|
||||
struct mavlink_subscriptions mavlink_subs;
|
||||
|
|
@ -99,6 +100,7 @@ static void l_vehicle_attitude(struct listener *l);
|
|||
static void l_vehicle_gps_position(struct listener *l);
|
||||
static void l_vehicle_status(struct listener *l);
|
||||
static void l_rc_channels(struct listener *l);
|
||||
static void l_input_rc(struct listener *l);
|
||||
static void l_global_position(struct listener *l);
|
||||
static void l_local_position(struct listener *l);
|
||||
static void l_global_position_setpoint(struct listener *l);
|
||||
|
|
@ -116,6 +118,7 @@ struct listener listeners[] = {
|
|||
{l_vehicle_gps_position, &mavlink_subs.gps_sub, 0},
|
||||
{l_vehicle_status, &status_sub, 0},
|
||||
{l_rc_channels, &rc_sub, 0},
|
||||
{l_input_rc, &mavlink_subs.input_rc_sub, 0},
|
||||
{l_global_position, &mavlink_subs.global_pos_sub, 0},
|
||||
{l_local_position, &mavlink_subs.local_pos_sub, 0},
|
||||
{l_global_position_setpoint, &mavlink_subs.spg_sub, 0},
|
||||
|
|
@ -274,21 +277,29 @@ l_rc_channels(struct listener *l)
|
|||
{
|
||||
/* copy rc channels into local buffer */
|
||||
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
|
||||
// XXX Add RC channels scaled message here
|
||||
}
|
||||
|
||||
void
|
||||
l_input_rc(struct listener *l)
|
||||
{
|
||||
/* copy rc channels into local buffer */
|
||||
orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
|
||||
|
||||
if (gcs_link)
|
||||
/* Channels are sent in MAVLink main loop at a fixed interval */
|
||||
mavlink_msg_rc_channels_raw_send(chan,
|
||||
rc.timestamp / 1000,
|
||||
rc_raw.timestamp / 1000,
|
||||
0,
|
||||
rc.chan[0].raw,
|
||||
rc.chan[1].raw,
|
||||
rc.chan[2].raw,
|
||||
rc.chan[3].raw,
|
||||
rc.chan[4].raw,
|
||||
rc.chan[5].raw,
|
||||
rc.chan[6].raw,
|
||||
rc.chan[7].raw,
|
||||
rc.rssi);
|
||||
(rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX,
|
||||
(rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX,
|
||||
(rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX,
|
||||
(rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX,
|
||||
(rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX,
|
||||
(rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX,
|
||||
(rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX,
|
||||
(rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX,
|
||||
255);
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -584,6 +595,10 @@ uorb_receive_start(void)
|
|||
rc_sub = orb_subscribe(ORB_ID(rc_channels));
|
||||
orb_set_interval(rc_sub, 100); /* 10Hz updates */
|
||||
|
||||
/* --- RC RAW VALUE --- */
|
||||
mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc));
|
||||
orb_set_interval(mavlink_subs.input_rc_sub, 100);
|
||||
|
||||
/* --- GLOBAL POS VALUE --- */
|
||||
mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */
|
||||
|
|
|
|||
|
|
@ -57,6 +57,7 @@
|
|||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
|
||||
struct mavlink_subscriptions {
|
||||
int sensor_sub;
|
||||
|
|
@ -75,6 +76,7 @@ struct mavlink_subscriptions {
|
|||
int spl_sub;
|
||||
int spg_sub;
|
||||
int debug_key_value;
|
||||
int input_rc_sub;
|
||||
};
|
||||
|
||||
extern struct mavlink_subscriptions mavlink_subs;
|
||||
|
|
|
|||
|
|
@ -58,11 +58,13 @@
|
|||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
|
||||
#include <systemlib/ppm_decode.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
|
|
@ -143,6 +145,7 @@ private:
|
|||
int _gyro_sub; /**< raw gyro data subscription */
|
||||
int _accel_sub; /**< raw accel data subscription */
|
||||
int _mag_sub; /**< raw mag data subscription */
|
||||
int _rc_sub; /**< raw rc channels data subscription */
|
||||
int _baro_sub; /**< raw baro data subscription */
|
||||
int _vstatus_sub; /**< vehicle status subscription */
|
||||
int _params_sub; /**< notification of parameter updates */
|
||||
|
|
@ -162,6 +165,7 @@ private:
|
|||
float rev[_rc_max_chan_count];
|
||||
float dz[_rc_max_chan_count];
|
||||
float ex[_rc_max_chan_count];
|
||||
float scaling_factor[_rc_max_chan_count];
|
||||
|
||||
float gyro_offset[3];
|
||||
float mag_offset[3];
|
||||
|
|
@ -331,6 +335,7 @@ Sensors::Sensors() :
|
|||
_gyro_sub(-1),
|
||||
_accel_sub(-1),
|
||||
_mag_sub(-1),
|
||||
_rc_sub(-1),
|
||||
_baro_sub(-1),
|
||||
_vstatus_sub(-1),
|
||||
_params_sub(-1),
|
||||
|
|
@ -464,14 +469,13 @@ Sensors::parameters_update()
|
|||
warnx("Failed getting exponential gain for chan %d", i);
|
||||
}
|
||||
|
||||
_rc.chan[i].scaling_factor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
|
||||
_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
|
||||
|
||||
/* handle blowup in the scaling factor calculation */
|
||||
if (isnan(_rc.chan[i].scaling_factor) || isinf(_rc.chan[i].scaling_factor)) {
|
||||
_rc.chan[i].scaling_factor = 0;
|
||||
if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) {
|
||||
_parameters.scaling_factor[i] = 0;
|
||||
}
|
||||
|
||||
_rc.chan[i].mid = _parameters.trim[i];
|
||||
}
|
||||
|
||||
/* update RC function mappings */
|
||||
|
|
@ -856,99 +860,126 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
|
|||
void
|
||||
Sensors::ppm_poll()
|
||||
{
|
||||
struct manual_control_setpoint_s manual_control;
|
||||
/* fake low-level driver, directly pulling from driver variables */
|
||||
static orb_advert_t rc_input_pub = -1;
|
||||
struct rc_input_values raw;
|
||||
|
||||
/* check to see whether a new frame has been decoded */
|
||||
if (_ppm_last_valid == ppm_last_valid_decode)
|
||||
return;
|
||||
/* require at least two chanels to consider the signal valid */
|
||||
if (ppm_decoded_channels < 4)
|
||||
return;
|
||||
raw.timestamp = ppm_last_valid_decode;
|
||||
|
||||
unsigned channel_limit = ppm_decoded_channels;
|
||||
if (channel_limit > _rc_max_chan_count)
|
||||
channel_limit = _rc_max_chan_count;
|
||||
if (ppm_decoded_channels > 1) {
|
||||
|
||||
/* we are accepting this decode */
|
||||
_ppm_last_valid = ppm_last_valid_decode;
|
||||
|
||||
/* Read out values from HRT */
|
||||
for (unsigned int i = 0; i < channel_limit; i++) {
|
||||
_rc.chan[i].raw = ppm_buffer[i];
|
||||
|
||||
/* scale around the mid point differently for lower and upper range */
|
||||
if (ppm_buffer[i] > (_parameters.trim[i] + _parameters.dz[i])) {
|
||||
_rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
|
||||
} else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) {
|
||||
/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
|
||||
_rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
|
||||
|
||||
} else {
|
||||
/* in the configured dead zone, output zero */
|
||||
_rc.chan[i].scaled = 0.0f;
|
||||
for (int i = 0; i < ppm_decoded_channels; i++) {
|
||||
raw.values[i] = ppm_buffer[i];
|
||||
}
|
||||
|
||||
/* reverse channel if required */
|
||||
if (i == _rc.function[THROTTLE]) {
|
||||
if ((int)_parameters.rev[i] == -1) {
|
||||
_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
|
||||
raw.channel_count = ppm_decoded_channels;
|
||||
|
||||
/* publish to object request broker */
|
||||
if (rc_input_pub <= 0) {
|
||||
rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw);
|
||||
} else {
|
||||
orb_publish(ORB_ID(input_rc), rc_input_pub, &raw);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
|
||||
bool rc_updated;
|
||||
orb_check(_rc_sub, &rc_updated);
|
||||
|
||||
if (rc_updated) {
|
||||
struct rc_input_values rc_input;
|
||||
|
||||
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
|
||||
|
||||
struct manual_control_setpoint_s manual_control;
|
||||
|
||||
/* require at least two chanels to consider the signal valid */
|
||||
if (rc_input.channel_count < 2)
|
||||
return;
|
||||
|
||||
unsigned channel_limit = rc_input.channel_count;
|
||||
if (channel_limit > _rc_max_chan_count)
|
||||
channel_limit = _rc_max_chan_count;
|
||||
|
||||
/* we are accepting this message */
|
||||
_ppm_last_valid = rc_input.timestamp;
|
||||
|
||||
/* Read out values from raw message */
|
||||
for (unsigned int i = 0; i < channel_limit; i++) {
|
||||
|
||||
/* scale around the mid point differently for lower and upper range */
|
||||
if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
|
||||
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
|
||||
} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
|
||||
/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
|
||||
_rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
|
||||
|
||||
} else {
|
||||
/* in the configured dead zone, output zero */
|
||||
_rc.chan[i].scaled = 0.0f;
|
||||
}
|
||||
} else {
|
||||
_rc.chan[i].scaled *= _parameters.rev[i];
|
||||
|
||||
/* reverse channel if required */
|
||||
if (i == _rc.function[THROTTLE]) {
|
||||
if ((int)_parameters.rev[i] == -1) {
|
||||
_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
|
||||
}
|
||||
} else {
|
||||
_rc.chan[i].scaled *= _parameters.rev[i];
|
||||
}
|
||||
|
||||
/* handle any parameter-induced blowups */
|
||||
if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled))
|
||||
_rc.chan[i].scaled = 0.0f;
|
||||
}
|
||||
|
||||
/* handle any parameter-induced blowups */
|
||||
if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled))
|
||||
_rc.chan[i].scaled = 0.0f;
|
||||
_rc.chan_count = rc_input.channel_count;
|
||||
_rc.timestamp = rc_input.timestamp;
|
||||
|
||||
//_rc.chan[i].scaled = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor;
|
||||
manual_control.timestamp = rc_input.timestamp;
|
||||
|
||||
/* roll input - rolling right is stick-wise and rotation-wise positive */
|
||||
manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
|
||||
if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
|
||||
if (manual_control.roll > 1.0f) manual_control.roll = 1.0f;
|
||||
if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) {
|
||||
manual_control.roll *= _parameters.rc_scale_roll;
|
||||
}
|
||||
|
||||
/*
|
||||
* pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
|
||||
* so reverse sign.
|
||||
*/
|
||||
manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled;
|
||||
if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
|
||||
if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f;
|
||||
if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) {
|
||||
manual_control.pitch *= _parameters.rc_scale_pitch;
|
||||
}
|
||||
|
||||
/* yaw input - stick right is positive and positive rotation */
|
||||
manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw;
|
||||
if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
|
||||
if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f;
|
||||
if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) {
|
||||
manual_control.yaw *= _parameters.rc_scale_yaw;
|
||||
}
|
||||
|
||||
/* throttle input */
|
||||
manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
|
||||
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
|
||||
if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
|
||||
|
||||
/* mode switch input */
|
||||
manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled;
|
||||
if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
|
||||
if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f;
|
||||
|
||||
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
|
||||
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
|
||||
}
|
||||
|
||||
_rc.chan_count = ppm_decoded_channels;
|
||||
_rc.timestamp = ppm_last_valid_decode;
|
||||
|
||||
manual_control.timestamp = ppm_last_valid_decode;
|
||||
|
||||
/* roll input - rolling right is stick-wise and rotation-wise positive */
|
||||
manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled;
|
||||
if (manual_control.roll < -1.0f) manual_control.roll = -1.0f;
|
||||
if (manual_control.roll > 1.0f) manual_control.roll = 1.0f;
|
||||
if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) {
|
||||
manual_control.roll *= _parameters.rc_scale_roll;
|
||||
}
|
||||
|
||||
/*
|
||||
* pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
|
||||
* so reverse sign.
|
||||
*/
|
||||
manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled;
|
||||
if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f;
|
||||
if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f;
|
||||
if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) {
|
||||
manual_control.pitch *= _parameters.rc_scale_pitch;
|
||||
}
|
||||
|
||||
/* yaw input - stick right is positive and positive rotation */
|
||||
manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw;
|
||||
if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
|
||||
if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f;
|
||||
if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) {
|
||||
manual_control.yaw *= _parameters.rc_scale_yaw;
|
||||
}
|
||||
|
||||
/* throttle input */
|
||||
manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
|
||||
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
|
||||
if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
|
||||
|
||||
/* mode switch input */
|
||||
manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled;
|
||||
if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
|
||||
if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f;
|
||||
|
||||
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
|
||||
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -979,6 +1010,7 @@ Sensors::task_main()
|
|||
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
|
||||
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
|
||||
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
|
||||
_rc_sub = orb_subscribe(ORB_ID(input_rc));
|
||||
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
|
||||
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
|
|
|
|||
|
|
@ -50,14 +50,6 @@
|
|||
* @{
|
||||
*/
|
||||
|
||||
enum RC_CHANNELS_STATUS
|
||||
{
|
||||
UNKNOWN = 0,
|
||||
KNOWN = 1,
|
||||
SIGNAL = 2,
|
||||
TIMEOUT = 3
|
||||
};
|
||||
|
||||
/**
|
||||
* This defines the mapping of the RC functions.
|
||||
* The value assigned to the specific function corresponds to the entry of
|
||||
|
|
@ -85,12 +77,7 @@ struct rc_channels_s {
|
|||
uint64_t timestamp; /**< In microseconds since boot time. */
|
||||
uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */
|
||||
struct {
|
||||
uint16_t mid; /**< midpoint (0). */
|
||||
float scaling_factor; /**< scaling factor from raw counts to -1..+1 */
|
||||
uint16_t raw; /**< current raw value */
|
||||
float scaled; /**< Scaled to -1..1 (throttle: 0..1) */
|
||||
uint16_t override;
|
||||
enum RC_CHANNELS_STATUS status; /**< status of the channel */
|
||||
} chan[RC_CHANNELS_FUNCTION_MAX];
|
||||
uint8_t chan_count; /**< maximum number of valid channels */
|
||||
|
||||
|
|
@ -98,6 +85,7 @@ struct rc_channels_s {
|
|||
char function_name[RC_CHANNELS_FUNCTION_MAX][20];
|
||||
uint8_t function[RC_CHANNELS_FUNCTION_MAX];
|
||||
uint8_t rssi; /**< Overall receive signal strength */
|
||||
bool is_valid; /**< Inputs are valid, no timeout */
|
||||
}; /**< radio control channels. */
|
||||
|
||||
/**
|
||||
|
|
|
|||
Loading…
Reference in New Issue