mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 00:28:30 -04:00
AP_Scripting: copter deadreckoning lua check fixes
This commit is contained in:
parent
3d39f9bb20
commit
e5be8e9bf0
@ -55,8 +55,6 @@
|
||||
-- a. SIM_WIND_DIR <-- sets direction wind is coming from
|
||||
-- b. SIM_WIND_SPD <-- sets wind speed in m/s
|
||||
--
|
||||
-- luacheck: only 0
|
||||
|
||||
|
||||
-- create and initialise parameters
|
||||
local PARAM_TABLE_KEY = 86 -- parameter table key must be used by only one script on a particular flight controller
|
||||
@ -194,7 +192,6 @@ local gps_or_ekf_bad = true -- true if GPS and/or EKF is bad, true once
|
||||
local flight_stage = 0 -- 0. wait for good-gps and dist-from-home, 1=wait for bad gps or ekf, 2=level vehicle, 3=deadreckon home
|
||||
local gps_bad_start_time_ms = 0 -- system time GPS quality went bad (0 if not bad)
|
||||
local recovery_start_time_ms = 0-- system time GPS quality and EKF failsafe recovered (0 if not recovered)
|
||||
local fly_start_time_ms = 0 -- system time fly using deadreckoning started
|
||||
|
||||
local home_dist = 0 -- distance to home in meters
|
||||
local home_yaw = 0 -- direction to home in degrees
|
||||
@ -224,7 +221,6 @@ function update()
|
||||
end
|
||||
|
||||
-- check GPS
|
||||
local gps_primary = gps:primary_sensor()
|
||||
local gps_speed_acc = gps:speed_accuracy(gps:primary_sensor())
|
||||
if gps_speed_acc == nil then
|
||||
gps_speed_acc = 99
|
||||
@ -251,7 +247,7 @@ function update()
|
||||
|
||||
-- check EKF failsafe
|
||||
local fs_ekf = vehicle:has_ekf_failsafed()
|
||||
if not (ekf_bad == fs_ekf) then
|
||||
if ekf_bad ~= fs_ekf then
|
||||
ekf_bad = fs_ekf
|
||||
end
|
||||
|
||||
@ -290,7 +286,6 @@ function update()
|
||||
-- reset flight_stage when disarmed
|
||||
if not arming:is_armed() then
|
||||
flight_stage = 0
|
||||
fly_start_time_ms = 0
|
||||
transition_start_time_ms = 0
|
||||
return update, interval_ms
|
||||
end
|
||||
|
Loading…
Reference in New Issue
Block a user