Set_output_pwm for servo_functions 51-66 (Classic RC failsafe functionality)

Hi,

first of all i know that what i am doing is out of the scope for the intended ardupilot use.
I basically want to program a LUA script that includes RC-Failsafe functionality with defined servo positions. (Details what i want to do at the end, as well as an example LUA code)

I have a servo set to function 57 (RCIN7) to just output the input 7 from the rc receiver.
I am using a LUA script to detect a failsave event. ( rc:has_valid_input() )

I this happens to be false for more than 1.5 seconds, i want the servo to get to a defined PWM value → Failsave functionality like we know from classic rc systems.

I tried 3 options but none are working for my needs.

local elevator_channel = SRV_Channels:find_channel(57)

SRV_Channels:set_output_pwm_chan_timeout(elevator_channel, 1700, 300)

this does not work. There is no error message, but it just does not do anything. Same problem when using the RCIN7_scaled (146) function instead of 57.

if i change the servo to an autopilot actuated servo function like 21 (Rudder) it works immediately. BUT i don’t want to have any commands comming in from the autopilot. → not working for my case

local RC7 = rc:get_channel(7)

RC7:set_override(1200)

This basically works like i need it. It “simulates” a different incomming rc channel value and changes the servo to the desired position.
The problem is that the rc:has_valid_input() function sees this as a valid rc input. → Lua Failsafe off → Servo goes to last position → still no real RC-input → Lua Failsafe on … just a loop and NOT staying at the desired FS position while no RC signal is comming.
Can i somehow + reliable access the REAL Failsafe state from the RC input?
(even the native ardupilot failsafe function does not realize this input is not really from RC receiver. However this might be by design and as intended for other usecases.)

Possible other ideas i did not try yet but it should work:
4)
I could have the servos each on a scripting funtion (94-109) and forward the inputs to the servo via the lua script all the time but if a failsafe event happens.
BUT I’m hessitant to have all functions controlled only via a LUA script all the time. I’m not sure about the speed, and especially reliability.

Is it simpler + more reliable to make an ardupilot C-code change that forwards all RCIN values to the scripting servo functions (94-109) if there is no failsafe state? (and failsafe values if there is a failsafe state?)

Thank you so much for helping me!

-Christian



What i want to achieve:

I want to have my multi flaped model using an ELRS RC system and an ardupilot for logging.
The complex mixing of these wings is theoretically possible by using an axtensive LUA script or heavily modifying the C-Code.
On the other hand last minute servo modifications are easier done on the transmitter during a competition.
→ Mixing is done in Transmitter

Then the data is sent up via ELRS 3.
I want to use an ardupilot FC to log all position and attitude data for later evaluation of the aircraft performance. (+ GPS, airspeed,…)
Since it is already there it makes sense to use it as a converter for ELRS to Servo PWM as well + all autopilot functions and telemetry are available to play around outside of competitions.
Everything works like i hoped but the failsafe functionality.


exemplary LUA code: yes i can reduce some lines.

-- start LUA script and send start message to GCS
gcs:send_text(5, 'LUA: Failsave script started')

-- initialize all used variables
local elevator_channel = SRV_Channels:find_channel(57)
local rudder_channel = SRV_Channels:find_channel(21)
local lost_signal_time = 0
local current_time = 0
local time_difference = 0
local RC7 = rc:get_channel(7)

-- function update that does all the failsave action
function update()
	
	-- check if the RC has a valid input
	if rc:has_valid_input() then
		-- if valid signal: do nothing except set the variable to zero
		-- this variable saves the last time the rc input was not valid
		lost_signal_time = 0
	else
		-- if the input is not valid = no signal from RC
		
		if lost_signal_time == 0 then
			-- save the first time when no signal was comming in
			lost_signal_time = millis()
		end
		
		-- get current time
		current_time = millis()
		-- calculate the time differrence from initial lost contact to now
		time_difference = current_time - lost_signal_time
		
		if time_difference >= 1500 then
			-- if this time difference is bigger than 1.5 seconds go into failsave condition
			gcs:send_text(5, 'LUA: NOT a valid rc input. set failsave position')
			
			-- Set the rudder channel to a fixed pwm. This is with servo function 21 Example 2) -> works
			SRV_Channels:set_output_pwm_chan_timeout(rudder_channel, 1700, 300)
			
			-- Set the elevator channel to a fixed pwm. This is with servo function 57 or 146 Example 1) -> Does NOT work!!!
			SRV_Channels:set_output_pwm_chan_timeout(elevator_channel, 1700, 300)
			
			-- Optional test. This works, but oscillates between failsafe state and non-failsafe state. Example 3)
			-- RC7:set_override(1200)
			
		end
	end
	
	-- Repeat function every 200ms. This is more frequent than the timeout -> As long as rc input is invalid, the pwm outputs should stay in FS position.
	return update, 200
	
end

-- start function update
return update()

Little hard to read your script, but the RC overrides must be repeatedly sent such that they don’t timeout. I think the binding supports a custom timeout argument, but there is probably an upper bound of a few seconds.

Off the top of my head, I think also that sending a PWM value of 0 immediately cancels an active override.

Hi,

thanks for the quick response.
i completely agree. I reformatted and annotated the code.

Basically if i could set the timeout of the override function to infinity the failsafe position would stay in and maybe when the RC signal is coming back it “overrides the overwride”.

I won’t be able to actively get it back via the LUA script because while the override is active, ardupilot thinks that it gets correct RC signals.

Indeed, that logic has inherent flaws, and I’m not entirely sure how to help there.

I apologize, I’m reading and replying on my phone. I think, if I understand correctly, you want to detect an RC failsafe via scripting. There is no binding for that, but we should have one. There is decent documentation for adding a binding, and I think a PR for an added RC failsafe binding would be well received, if you’re inclined to dive into the source code.

i will have a look if i am able to do it. It will however take some time.

Thank you!!!