Browse Source

commander: check center throttle for POSCTL/ALTCTL

When flying POSCTL and ALTCTL the throttle stick is usually spring
loaded and therefore centered. Therefore, it makes more sense to check
for above center instead of above low.
sbg
Julian Oes 5 years ago
parent
commit
87415d36a2
  1. 28
      src/modules/commander/Commander.cpp

28
src/modules/commander/Commander.cpp

@ -837,17 +837,23 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
break; break;
} }
// Refuse to arm if in manual with non-zero throttle const bool throttle_above_low = (sp_man.z > 0.1f);
if (cmd_arms const bool throttle_above_center = (sp_man.z > 0.6f);
&& (status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL
|| status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL if (cmd_arms && throttle_above_center &&
|| status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL (status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL ||
|| status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)) {
|| status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_STAB mavlink_log_critical(&mavlink_log_pub, "Arming denied! Throttle not centered");
|| status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE) cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
&& (sp_man.z > 0.1f)) { break;
}
mavlink_log_critical(&mavlink_log_pub, "Arming denied! Manual throttle not zero");
if (cmd_arms && throttle_above_low &&
(status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL ||
status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO ||
status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_STAB ||
status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE)) {
mavlink_log_critical(&mavlink_log_pub, "Arming denied! Throttle not zero");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break; break;
} }

Loading…
Cancel
Save