CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

simple_offboard: correctly check manual control timeout, separate it from kill switch check

Browse Source
This commit is contained in:
Oleg Kalachev
2020-10-22 19:11:57 +03:00
parent 039d2438cd
commit 6c8291749f
1 changed files with 14 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
clover/src/simple_offboard.cpp
View File

@@ -73,6 +73,7 @@ ros::Duration state_timeout;
ros::Duration velocity_timeout;
ros::Duration global_position_timeout;
ros::Duration battery_timeout;
ros::Duration manual_control_timeout;
float default_speed;
bool auto_release;
bool land_only_in_offboard, nav_from_sp, check_kill_switch;
@@ -488,16 +489,19 @@ void publishSetpoint(const ros::TimerEvent& event)
publish(event.current_real);
}
inline void checkKillSwitch()
inline void checkManualControl()
{
if (!TIMEOUT(manual_control, state_timeout))
throw std::runtime_error("Manual control timeout, can't check kill switch status");
if (!manual_control_timeout.isZero() && TIMEOUT(manual_control, manual_control_timeout)) {
throw std::runtime_error("Manual control timeout, RC is switched off?");
}
const int KILL_SWITCH_BIT = 12; // https://github.com/PX4/Firmware/blob/c302514a0809b1765fafd13c014d705446ae1113/src/modules/mavlink/mavlink_messages.cpp#L3975
bool kill_switch = manual_control.buttons & (1 << KILL_SWITCH_BIT);
if (check_kill_switch) {
const int KILL_SWITCH_BIT = 12; // https://github.com/PX4/Firmware/blob/c302514a0809b1765fafd13c014d705446ae1113/src/modules/mavlink/mavlink_messages.cpp#L3975
bool kill_switch = manual_control.buttons & (1 << KILL_SWITCH_BIT);
if (kill_switch)
throw std::runtime_error("Kill switch is on");
if (kill_switch)
throw std::runtime_error("Kill switch is on");
}
}
inline void checkState()
@@ -527,8 +531,8 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
// Checks
checkState();
if (auto_arm && check_kill_switch) {
checkKillSwitch();
if (auto_arm) {
checkManualControl();
}
// default frame is local frame
@@ -862,6 +866,7 @@ int main(int argc, char **argv)
velocity_timeout = ros::Duration(nh_priv.param("velocity_timeout", 2.0));
global_position_timeout = ros::Duration(nh_priv.param("global_position_timeout", 10.0));
battery_timeout = ros::Duration(nh_priv.param("battery_timeout", 2.0));
manual_control_timeout = ros::Duration(nh_priv.param("manual_control_timeout", 0.0));
transform_timeout = ros::Duration(nh_priv.param("transform_timeout", 0.5));
telemetry_transform_timeout = ros::Duration(nh_priv.param("telemetry_transform_timeout", 0.5));