forked from Archive/PX4-Autopilot
sensors: use enum for switches position and -1..1 for values in 'manual_control_setpoint' topic
This commit is contained in:
parent
2c4792d48e
commit
1d5f62d890
|
@ -135,7 +135,7 @@
|
|||
*/
|
||||
#define PCB_TEMP_ESTIMATE_DEG 5.0f
|
||||
|
||||
#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
|
||||
#define STICK_ON_OFF_LIMIT 0.75f
|
||||
|
||||
/**
|
||||
* Sensor app start / stop handling function
|
||||
|
@ -169,6 +169,16 @@ private:
|
|||
|
||||
hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */
|
||||
|
||||
/**
|
||||
* Get and limit value for specified RC function. Returns NAN if not mapped.
|
||||
*/
|
||||
float get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value);
|
||||
|
||||
/**
|
||||
* Get switch position for specified function.
|
||||
*/
|
||||
switch_pos_t get_rc_switch_position(enum RC_CHANNELS_FUNCTION func);
|
||||
|
||||
/**
|
||||
* Gather and publish RC input data.
|
||||
*/
|
||||
|
@ -1275,6 +1285,45 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
|
|||
}
|
||||
}
|
||||
|
||||
float
|
||||
Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value)
|
||||
{
|
||||
if (_rc.function[func] >= 0) {
|
||||
float value = _rc.chan[_rc.function[func]].scaled;
|
||||
if (value < min_value) {
|
||||
return min_value;
|
||||
|
||||
} else if (value > max_value) {
|
||||
return max_value;
|
||||
|
||||
} else {
|
||||
return value;
|
||||
}
|
||||
} else {
|
||||
return NAN;
|
||||
}
|
||||
}
|
||||
|
||||
switch_pos_t
|
||||
Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func)
|
||||
{
|
||||
if (_rc.function[func] >= 0) {
|
||||
float value = _rc.chan[_rc.function[func]].scaled;
|
||||
if (value > STICK_ON_OFF_LIMIT) {
|
||||
return SWITCH_POS_ON;
|
||||
|
||||
} else if (value < STICK_ON_OFF_LIMIT) {
|
||||
return SWITCH_POS_OFF;
|
||||
|
||||
} else {
|
||||
return SWITCH_POS_MIDDLE;
|
||||
}
|
||||
|
||||
} else {
|
||||
return SWITCH_POS_NONE;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Sensors::rc_poll()
|
||||
{
|
||||
|
@ -1292,13 +1341,6 @@ Sensors::rc_poll()
|
|||
manual_control.pitch = NAN;
|
||||
manual_control.yaw = NAN;
|
||||
manual_control.throttle = NAN;
|
||||
|
||||
manual_control.mode_switch = NAN;
|
||||
manual_control.return_switch = NAN;
|
||||
manual_control.assisted_switch = NAN;
|
||||
manual_control.mission_switch = NAN;
|
||||
// manual_control.auto_offboard_input_switch = NAN;
|
||||
|
||||
manual_control.flaps = NAN;
|
||||
manual_control.aux1 = NAN;
|
||||
manual_control.aux2 = NAN;
|
||||
|
@ -1306,6 +1348,11 @@ Sensors::rc_poll()
|
|||
manual_control.aux4 = NAN;
|
||||
manual_control.aux5 = NAN;
|
||||
|
||||
manual_control.mode_switch = SWITCH_POS_NONE;
|
||||
manual_control.return_switch = SWITCH_POS_NONE;
|
||||
manual_control.assisted_switch = SWITCH_POS_NONE;
|
||||
manual_control.mission_switch = SWITCH_POS_NONE;
|
||||
|
||||
manual_control.signal_lost = true;
|
||||
|
||||
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
|
||||
|
@ -1316,7 +1363,7 @@ Sensors::rc_poll()
|
|||
/* signal looks good */
|
||||
manual_control.signal_lost = false;
|
||||
|
||||
/* check throttle failsafe */
|
||||
/* check for throttle failsafe */
|
||||
if (_parameters.rc_fs_ch != 0) {
|
||||
if (_parameters.rc_fs_mode == 0) {
|
||||
if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) {
|
||||
|
@ -1397,89 +1444,23 @@ Sensors::rc_poll()
|
|||
if (!manual_control.signal_lost) {
|
||||
/* fill values in manual_control_setpoint topic only if signal is valid */
|
||||
|
||||
/* roll input - rolling right is stick-wise and rotation-wise positive */
|
||||
manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
|
||||
/*
|
||||
* pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
|
||||
* so reverse sign.
|
||||
*/
|
||||
manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled);
|
||||
/* yaw input - stick right is positive and positive rotation */
|
||||
manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled);
|
||||
/* throttle input */
|
||||
manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
|
||||
/* limit controls */
|
||||
manual_control.roll = get_rc_value(ROLL, -1.0, 1.0);
|
||||
manual_control.pitch = get_rc_value(PITCH, -1.0, 1.0);
|
||||
manual_control.yaw = get_rc_value(YAW, -1.0, 1.0);
|
||||
manual_control.throttle = get_rc_value(THROTTLE, 0.0, 1.0);
|
||||
manual_control.flaps = get_rc_value(FLAPS, -1.0, 1.0);
|
||||
manual_control.aux1 = get_rc_value(AUX_1, -1.0, 1.0);
|
||||
manual_control.aux2 = get_rc_value(AUX_2, -1.0, 1.0);
|
||||
manual_control.aux3 = get_rc_value(AUX_3, -1.0, 1.0);
|
||||
manual_control.aux4 = get_rc_value(AUX_4, -1.0, 1.0);
|
||||
manual_control.aux5 = get_rc_value(AUX_5, -1.0, 1.0);
|
||||
|
||||
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
|
||||
|
||||
if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
|
||||
|
||||
/* scale output */
|
||||
if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) {
|
||||
manual_control.roll *= _parameters.rc_scale_roll;
|
||||
}
|
||||
|
||||
if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) {
|
||||
manual_control.pitch *= _parameters.rc_scale_pitch;
|
||||
}
|
||||
|
||||
if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) {
|
||||
manual_control.yaw *= _parameters.rc_scale_yaw;
|
||||
}
|
||||
|
||||
/* flaps */
|
||||
if (_rc.function[FLAPS] >= 0) {
|
||||
|
||||
manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled);
|
||||
|
||||
if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) {
|
||||
manual_control.flaps *= _parameters.rc_scale_flaps;
|
||||
}
|
||||
}
|
||||
|
||||
/* mode switch input */
|
||||
if (_rc.function[MODE] >= 0) {
|
||||
manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
|
||||
}
|
||||
|
||||
/* assisted switch input */
|
||||
if (_rc.function[ASSISTED] >= 0) {
|
||||
manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
|
||||
}
|
||||
|
||||
/* mission switch input */
|
||||
if (_rc.function[MISSION] >= 0) {
|
||||
manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
|
||||
}
|
||||
|
||||
/* return switch input */
|
||||
if (_rc.function[RETURN] >= 0) {
|
||||
manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
|
||||
}
|
||||
|
||||
// if (_rc.function[OFFBOARD_MODE] >= 0) {
|
||||
// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
|
||||
// }
|
||||
|
||||
/* aux functions, only assign if valid mapping is present */
|
||||
if (_rc.function[AUX_1] >= 0) {
|
||||
manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled);
|
||||
}
|
||||
|
||||
if (_rc.function[AUX_2] >= 0) {
|
||||
manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled);
|
||||
}
|
||||
|
||||
if (_rc.function[AUX_3] >= 0) {
|
||||
manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled);
|
||||
}
|
||||
|
||||
if (_rc.function[AUX_4] >= 0) {
|
||||
manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled);
|
||||
}
|
||||
|
||||
if (_rc.function[AUX_5] >= 0) {
|
||||
manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
|
||||
}
|
||||
/* mode switches */
|
||||
manual_control.mode_switch = get_rc_switch_position(MODE);
|
||||
manual_control.assisted_switch = get_rc_switch_position(ASSISTED);
|
||||
manual_control.mission_switch = get_rc_switch_position(MISSION);
|
||||
manual_control.return_switch = get_rc_switch_position(RETURN);
|
||||
|
||||
/* copy from mapped manual control to control group 3 */
|
||||
actuator_group_3.control[0] = manual_control.roll;
|
||||
|
|
|
@ -43,6 +43,16 @@
|
|||
#include <stdint.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* Switch position
|
||||
*/
|
||||
typedef enum {
|
||||
SWITCH_POS_NONE = 0, /**< switch is not mapped */
|
||||
SWITCH_POS_ON, /**< switch activated (value = 1) */
|
||||
SWITCH_POS_MIDDLE, /**< middle position (value = 0) */
|
||||
SWITCH_POS_OFF /**< switch not activated (value = -1) */
|
||||
} switch_pos_t;
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
|
@ -53,32 +63,25 @@ struct manual_control_setpoint_s {
|
|||
|
||||
bool signal_lost; /**< control signal lost, should be checked together with topic timeout */
|
||||
|
||||
float roll; /**< ailerons roll / roll rate input */
|
||||
float pitch; /**< elevator / pitch / pitch rate */
|
||||
float yaw; /**< rudder / yaw rate / yaw */
|
||||
float throttle; /**< throttle / collective thrust / altitude */
|
||||
|
||||
float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */
|
||||
float return_switch; /**< land 2 position switch (mandatory): land, no effect */
|
||||
float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */
|
||||
float mission_switch; /**< mission 2 position switch (optional): mission, loiter */
|
||||
|
||||
/**
|
||||
* Any of the channels below may not be available and be set to NaN
|
||||
* Any of the channels may not be available and be set to NaN
|
||||
* to indicate that it does not contain valid data.
|
||||
*/
|
||||
|
||||
// XXX needed or parameter?
|
||||
//float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */
|
||||
|
||||
float flaps; /**< flap position */
|
||||
|
||||
float roll; /**< ailerons roll / roll rate input, -1..1 */
|
||||
float pitch; /**< elevator / pitch / pitch rate, -1..1 */
|
||||
float yaw; /**< rudder / yaw rate / yaw, -1..1 */
|
||||
float throttle; /**< throttle / collective thrust / altitude, 0..1 */
|
||||
float flaps; /**< flap position */
|
||||
float aux1; /**< default function: camera yaw / azimuth */
|
||||
float aux2; /**< default function: camera pitch / tilt */
|
||||
float aux3; /**< default function: camera trigger */
|
||||
float aux4; /**< default function: camera roll */
|
||||
float aux5; /**< default function: payload drop */
|
||||
|
||||
switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */
|
||||
switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */
|
||||
switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */
|
||||
switch_pos_t mission_switch; /**< mission 2 position switch (optional): mission, loiter */
|
||||
}; /**< manual control inputs */
|
||||
|
||||
/**
|
||||
|
|
Loading…
Reference in New Issue