|
|
|
@ -3,7 +3,6 @@
@@ -3,7 +3,6 @@
|
|
|
|
|
#include "Sub.h" |
|
|
|
|
|
|
|
|
|
namespace { |
|
|
|
|
static uint32_t last_stabilize_message_ms = 0; |
|
|
|
|
uint32_t last_pilot_heading; |
|
|
|
|
uint32_t last_pilot_yaw_input_ms = 0; |
|
|
|
|
} |
|
|
|
@ -65,11 +64,6 @@ void Sub::stabilize_run()
@@ -65,11 +64,6 @@ void Sub::stabilize_run()
|
|
|
|
|
|
|
|
|
|
} else { // hold current heading
|
|
|
|
|
|
|
|
|
|
if(tnow > last_stabilize_message_ms + 1500) { |
|
|
|
|
gcs_send_text_fmt(MAV_SEVERITY_INFO, "target: %d", last_pilot_heading); |
|
|
|
|
last_stabilize_message_ms = tnow; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// this check is required to prevent bounce back after very fast yaw maneuvers
|
|
|
|
|
// the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
|
|
|
|
|
if(tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
|
|
|
|
|