Direct motor control by LUA commands

Salex · March 5, 2025, 3:39pm

I’d like to implement my own rate control loop, and set direct command to the motors for hovering and basic movements.
My testing is currently on SITL only, ArduCopter mode.
I calculate altitude and attitude rate errors, and then build it into motor command, and finally i set the value to the motor.

local HOVER_THROTTLE = 1500
local motor1_pwm = HOVER_THROTTLE + altitude_output + pitch_rate_output - roll_rate_output - yaw_rate_output + gravity_offset
local motor2_pwm = HOVER_THROTTLE + altitude_output - pitch_rate_output - roll_rate_output + yaw_rate_output + gravity_offset
local motor3_pwm = HOVER_THROTTLE + altitude_output - pitch_rate_output + roll_rate_output - yaw_rate_output + gravity_offset
local motor4_pwm = HOVER_THROTTLE + altitude_output + pitch_rate_output + roll_rate_output + yaw_rate_output + gravity_offset
  
-- Limit motor outputs
motor1_pwm = math.floor(math.max(1000, math.min(2000, motor1_pwm)))
motor2_pwm = math.floor(math.max(1000, math.min(2000, motor2_pwm)))
motor3_pwm = math.floor(math.max(1000, math.min(2000, motor3_pwm)))
motor4_pwm = math.floor(math.max(1000, math.min(2000, motor4_pwm)))

-- Set PWM outputs
SRV_Channels:set_output_pwm(33, math.floor(motor1_pwm))
SRV_Channels:set_output_pwm(34, math.floor(motor2_pwm))
SRV_Channels:set_output_pwm(35, math.floor(motor3_pwm))
SRV_Channels:set_output_pwm(36, math.floor(motor4_pwm))

I tried to work in GUIDED and in ACRO, but it looks like the copter is ignoring my PWM commands.
In what mode should i work in order to not interfere with RC or other commands ?

Yuri_Rage · March 5, 2025, 4:06pm

Your approach is likely flawed, and I don’t think you’ll be able to directly influence motor output the way you are attempting.

If you really want a custom rate controller, you need to look at modifying the C++ source with a custom flight mode (or possibly even deeper customization).

But what is it that you’re trying to do that guided mode will not accomplish?

Salex · March 5, 2025, 7:41pm

Hi Yuri,
This is a kind of educational project of autonomous racing drone, which implements rate control algorithm.
We would like to base it on standard ArduPilot and do not change its code because of the regulations.
The alternative approach may be to override RC commands in Acro, like with Mavlink, but i think it less effective, that’s why i thought about direct motor control.
The best option could be direct access to the AP’s rate controller, but as far as i know it is not possible from LUA.
Can you recommend another way to do this?

Yuri_Rage · March 5, 2025, 7:53pm

It’s still unclear why you’d need such low level access to the inner loop, and Lua is ill suited to such a task.

Guided mode seems more applicable for a racing algorithm so far as I can see.

manavgandhi17 · March 5, 2025, 9:17pm

You need to use the override command to override the autopilot:

But like Yuri is saying - Lua will not give you the rate needed for raw thruster control.

Salex · March 5, 2025, 10:34pm

I agree, it is not the optimal solution, but like i said, this is educational school project.
Can i achieve 150-200Hz update rate on LUA with H7 (480 MHz) cpu?
Can i pass commands for roll and pitch rates in Guided mode from LUA ?
I need fact response like in Acro mode.

Salex · March 5, 2025, 10:35pm

Thank you.
I will try this.

Yuri_Rage · March 5, 2025, 10:43pm

You’ll be lucky to get 100Hz. And if you just want to send attitude/rate commands, guided mode is literally designed for that type of behavior.

Don’t bother with servo overrides. You’ll crash the copter.

The following excerpt from bindings.desc shows most of what’s available to you in guided mode. You preface each with vehicle:.

singleton AP_Vehicle method set_target_location boolean Location
singleton AP_Vehicle method get_target_location boolean Location'Null
singleton AP_Vehicle method update_target_location boolean Location Location
singleton AP_Vehicle method set_target_pos_NED boolean Vector3f boolean float -360 +360 boolean float'skip_check boolean boolean
singleton AP_Vehicle method set_target_posvel_NED boolean Vector3f Vector3f
singleton AP_Vehicle method set_target_posvelaccel_NED boolean Vector3f Vector3f Vector3f boolean float -360 +360 boolean float'skip_check boolean
singleton AP_Vehicle method set_target_velaccel_NED boolean Vector3f Vector3f boolean float -360 +360 boolean float'skip_check boolean
singleton AP_Vehicle method set_target_velocity_NED boolean Vector3f
singleton AP_Vehicle method set_target_angle_and_climbrate boolean float -180 180 float -90 90 float -360 360 float'skip_check boolean float'skip_check
singleton AP_Vehicle method get_circle_radius boolean float'Null
singleton AP_Vehicle method set_circle_rate boolean float'skip_check
singleton AP_Vehicle method set_steering_and_throttle boolean float -1 1 float -1 1
singleton AP_Vehicle method get_steering_and_throttle boolean float'Null float'Null
singleton AP_Vehicle method get_wp_distance_m boolean float'Null
singleton AP_Vehicle method get_wp_bearing_deg boolean float'Null
singleton AP_Vehicle method get_wp_crosstrack_error_m boolean float'Null
singleton AP_Vehicle method get_pan_tilt_norm boolean float'Null float'Null
singleton AP_Vehicle method nav_script_time boolean uint16_t'Null uint8_t'Null float'Null float'Null int16_t'Null int16_t'Null
singleton AP_Vehicle method nav_script_time_done void uint16_t'skip_check
singleton AP_Vehicle method set_target_throttle_rate_rpy void float -100 100 float'skip_check float'skip_check float'skip_check
singleton AP_Vehicle method set_rudder_offset void float'skip_check boolean
singleton AP_Vehicle method set_desired_turn_rate_and_speed boolean float'skip_check float'skip_check
singleton AP_Vehicle method set_desired_speed boolean float'skip_check

You may find this link helpful, also:

github.com

ArduPilot/ardupilot/blob/Copter-4.5/libraries/AP_Scripting/docs/docs.lua

-- ArduPilot lua scripting documentation in EmmyLua Annotations
-- This file should be auto generated and then manual edited
-- generate with --scripting-docs, eg  ./waf copter --scripting-docs
-- see: https://github.com/sumneko/lua-language-server/wiki/EmmyLua-Annotations
-- luacheck: ignore 121 (Setting a read-only global variable)
-- luacheck: ignore 122 (Setting a read-only field of a global variable)
-- luacheck: ignore 212 (Unused argument)
-- luacheck: ignore 241 (Local variable is mutated but never accessed)

-- set and get for field types share function names
---@diagnostic disable: duplicate-set-field
---@diagnostic disable: missing-return

-- manual bindings

---@class uint32_t_ud
local uint32_t_ud = {}

-- create uint32_t_ud with optional value
---@param value? number|integer

This file has been truncated. show original

Salex · March 6, 2025, 9:58pm

I tried the function **set_target_throttle_rate_rpy ** from your list,
in SITL in Guided mode, with only pitch rate of 0.5 rad/s, the other arguments were zero, but nothing happened.
The copter keeps hovering on the same place at 20 m.
Did i forget something?

Yuri_Rage · March 6, 2025, 10:02pm

You need to continually send guided mode target commands at a rate of at least several Hz. If guided mode does not see this, it will assume a broken control scheme and simply loiter.

Salex · March 7, 2025, 9:33am

I’m sending it at 50 Hz.
Still nothing.
May be rate commands are also disabled for LUA like with Mavlink?

Salex · March 7, 2025, 5:37pm

I tested other function from the list set_target_rate_and_throttle and it is working.
The copter got pitch rate and moved forward as expected, but i need custom control over the throttle.
Can it be some bug in the implementation of set_target_throttle_rate_rpy ?
I didn’t found its source code in the AP.

Yuri_Rage · March 7, 2025, 5:44pm

I’ve not used the method you’re claiming as problematic, but both of these bindings look fairly new based on release notes and branch comparison.

If set_target_rate_and_throttle is working, then stick with it, and raise a GitHub issue regarding set_target_throttle_rate_rpy.

Salex · March 13, 2025, 8:23am

Thank you for the help!