2024-10-11 20:56:08 -03:00
|
|
|
-- switch between EKF2 and EKF3 on a switch
|
2021-08-14 21:35:15 -03:00
|
|
|
|
2024-10-11 20:56:08 -03:00
|
|
|
local AUX_FUNCTION_NUM = 300
|
2022-09-12 20:39:03 -03:00
|
|
|
local EKF_TYPE = Parameter('AHRS_EKF_TYPE')
|
2021-08-14 21:35:15 -03:00
|
|
|
|
|
|
|
function update()
|
2024-10-11 20:56:08 -03:00
|
|
|
local sw_pos = rc:get_aux_cached(AUX_FUNCTION_NUM)
|
|
|
|
if not sw_pos then
|
|
|
|
return update, 100
|
|
|
|
end
|
|
|
|
if sw_pos == 2 then
|
2021-08-14 21:35:15 -03:00
|
|
|
EKF_TYPE:set(3)
|
|
|
|
else
|
2024-10-11 20:56:08 -03:00
|
|
|
EKF_TYPE:set(2)
|
2021-08-14 21:35:15 -03:00
|
|
|
end
|
|
|
|
return update, 100
|
|
|
|
end
|
|
|
|
|
2024-10-11 20:56:08 -03:00
|
|
|
gcs:send_text(0, "Loaded AHRS switch for EKF3/EKF2")
|
|
|
|
|
2021-08-14 21:35:15 -03:00
|
|
|
return update()
|