forked from Archive/PX4-Autopilot
'signal_lost' flag added to manual_control_setpoint and rc_channels topics to indicate signal loss immediately
This commit is contained in:
parent
0205eebaa6
commit
367ce63b86
|
@ -118,8 +118,7 @@ extern struct system_load_s system_load;
|
|||
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
|
||||
|
||||
#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
|
||||
#define RC_TIMEOUT 100000
|
||||
#define RC_TIMEOUT_HIL 500000
|
||||
#define RC_TIMEOUT 500000
|
||||
#define DIFFPRESS_TIMEOUT 2000000
|
||||
|
||||
#define PRINT_INTERVAL 5000000
|
||||
|
@ -1109,16 +1108,8 @@ int commander_thread_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* XXX workaround:
|
||||
* Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz)
|
||||
* which can trigger RC loss if the computer/simulator lags.
|
||||
*/
|
||||
uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT;
|
||||
|
||||
/* start RC input check */
|
||||
if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) {
|
||||
if (!status.rc_input_blocked && !sp_man.signal_lost && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
|
||||
/* handle the case where RC signal was regained */
|
||||
if (!status.rc_signal_found_once) {
|
||||
status.rc_signal_found_once = true;
|
||||
|
|
|
@ -474,6 +474,7 @@ Sensors::Sensors() :
|
|||
_battery_discharged(0),
|
||||
_battery_current_timestamp(0)
|
||||
{
|
||||
memset(&_rc, 0, sizeof(_rc));
|
||||
|
||||
/* basic r/c parameters */
|
||||
for (unsigned i = 0; i < _rc_max_chan_count; i++) {
|
||||
|
@ -1283,12 +1284,6 @@ Sensors::rc_poll()
|
|||
if (rc_updated) {
|
||||
/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
|
||||
struct rc_input_values rc_input;
|
||||
|
||||
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
|
||||
|
||||
if (rc_input.rc_lost)
|
||||
return;
|
||||
|
||||
struct manual_control_setpoint_s manual_control;
|
||||
struct actuator_controls_s actuator_group_3;
|
||||
|
||||
|
@ -1311,19 +1306,28 @@ Sensors::rc_poll()
|
|||
manual_control.aux4 = NAN;
|
||||
manual_control.aux5 = NAN;
|
||||
|
||||
/* require at least four channels to consider the signal valid */
|
||||
if (rc_input.channel_count < 4)
|
||||
return;
|
||||
manual_control.signal_lost = true;
|
||||
|
||||
/* failsafe check */
|
||||
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)
|
||||
return;
|
||||
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
|
||||
|
||||
} else if (_parameters.rc_fs_mode == 1) {
|
||||
if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr)
|
||||
return;
|
||||
/* detect RC signal loss */
|
||||
/* check flags and require at least four channels to consider the signal valid */
|
||||
if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) {
|
||||
/* signal looks good */
|
||||
manual_control.signal_lost = false;
|
||||
|
||||
/* check 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) {
|
||||
manual_control.signal_lost = true;
|
||||
}
|
||||
|
||||
} else if (_parameters.rc_fs_mode == 1) {
|
||||
if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) {
|
||||
manual_control.signal_lost = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1332,10 +1336,7 @@ Sensors::rc_poll()
|
|||
if (channel_limit > _rc_max_chan_count)
|
||||
channel_limit = _rc_max_chan_count;
|
||||
|
||||
/* we are accepting this message */
|
||||
_rc_last_valid = rc_input.timestamp_last_signal;
|
||||
|
||||
/* Read out values from raw message */
|
||||
/* read out and scale values from raw message even if signal is invalid */
|
||||
for (unsigned int i = 0; i < channel_limit; i++) {
|
||||
|
||||
/*
|
||||
|
@ -1382,105 +1383,124 @@ Sensors::rc_poll()
|
|||
}
|
||||
|
||||
_rc.chan_count = rc_input.channel_count;
|
||||
_rc.timestamp = rc_input.timestamp_last_signal;
|
||||
_rc.rssi = rc_input.rssi;
|
||||
_rc.signal_lost = manual_control.signal_lost;
|
||||
|
||||
manual_control.timestamp = rc_input.timestamp_last_signal;
|
||||
|
||||
/* 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;
|
||||
|
||||
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 (!manual_control.signal_lost) {
|
||||
_rc_last_valid = rc_input.timestamp_last_signal;
|
||||
}
|
||||
|
||||
if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) {
|
||||
manual_control.pitch *= _parameters.rc_scale_pitch;
|
||||
}
|
||||
_rc.timestamp = _rc_last_valid;
|
||||
|
||||
if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) {
|
||||
manual_control.yaw *= _parameters.rc_scale_yaw;
|
||||
}
|
||||
manual_control.timestamp = _rc_last_valid;
|
||||
|
||||
/* flaps */
|
||||
if (_rc.function[FLAPS] >= 0) {
|
||||
if (!manual_control.signal_lost) {
|
||||
/* fill values in manual_control_setpoint topic only if signal is valid */
|
||||
|
||||
manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled);
|
||||
/* 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;
|
||||
|
||||
if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) {
|
||||
manual_control.flaps *= _parameters.rc_scale_flaps;
|
||||
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);
|
||||
}
|
||||
|
||||
/* copy from mapped manual control to control group 3 */
|
||||
actuator_group_3.control[0] = manual_control.roll;
|
||||
actuator_group_3.control[1] = manual_control.pitch;
|
||||
actuator_group_3.control[2] = manual_control.yaw;
|
||||
actuator_group_3.control[3] = manual_control.throttle;
|
||||
actuator_group_3.control[4] = manual_control.flaps;
|
||||
actuator_group_3.control[5] = manual_control.aux1;
|
||||
actuator_group_3.control[6] = manual_control.aux2;
|
||||
actuator_group_3.control[7] = manual_control.aux3;
|
||||
|
||||
/* publish actuator_controls_3 topic only if control signal is valid */
|
||||
if (_actuator_group_3_pub > 0) {
|
||||
orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
|
||||
|
||||
} else {
|
||||
_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
|
||||
}
|
||||
}
|
||||
|
||||
/* 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);
|
||||
}
|
||||
|
||||
/* copy from mapped manual control to control group 3 */
|
||||
actuator_group_3.control[0] = manual_control.roll;
|
||||
actuator_group_3.control[1] = manual_control.pitch;
|
||||
actuator_group_3.control[2] = manual_control.yaw;
|
||||
actuator_group_3.control[3] = manual_control.throttle;
|
||||
actuator_group_3.control[4] = manual_control.flaps;
|
||||
actuator_group_3.control[5] = manual_control.aux1;
|
||||
actuator_group_3.control[6] = manual_control.aux2;
|
||||
actuator_group_3.control[7] = manual_control.aux3;
|
||||
|
||||
/* check if ready for publishing */
|
||||
/* publish rc_channels topic even if signal is invalid, for debug */
|
||||
if (_rc_pub > 0) {
|
||||
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
|
||||
|
||||
|
@ -1489,21 +1509,13 @@ Sensors::rc_poll()
|
|||
_rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc);
|
||||
}
|
||||
|
||||
/* check if ready for publishing */
|
||||
/* publish manual_control_setpoint topic even if signal is not valid to update 'signal_lost' flag */
|
||||
if (_manual_control_pub > 0) {
|
||||
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
|
||||
|
||||
} else {
|
||||
_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
|
||||
}
|
||||
|
||||
/* check if ready for publishing */
|
||||
if (_actuator_group_3_pub > 0) {
|
||||
orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
|
||||
|
||||
} else {
|
||||
_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
@ -51,6 +51,8 @@
|
|||
struct manual_control_setpoint_s {
|
||||
uint64_t timestamp;
|
||||
|
||||
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 */
|
||||
|
|
|
@ -94,6 +94,7 @@ struct rc_channels_s {
|
|||
char function_name[RC_CHANNELS_FUNCTION_MAX][20];
|
||||
int8_t function[RC_CHANNELS_FUNCTION_MAX];
|
||||
uint8_t rssi; /**< Overall receive signal strength */
|
||||
bool signal_lost; /**< control signal lost, should be checked together with topic timeout */
|
||||
}; /**< radio control channels. */
|
||||
|
||||
/**
|
||||
|
|
Loading…
Reference in New Issue