mavlink / MAVSDK-Python

MAVSDK client for Python.
https://mavsdk.mavlink.io
BSD 3-Clause "New" or "Revised" License
311 stars 218 forks source link

Cannot control motors, always spinning at max speed #658

Open ilysec opened 6 months ago

ilysec commented 6 months ago

Hello,

I am working on a custom BlueROV2 airframe with 4 motors. I am trying to control the thrusts on each motor, however regardless of what I try, the motors arm and start to spin at max speed. I know that the wirings and setup are correct, since I can successfully test the throttle range in QGC's "Actuator Testing".

import asyncio
from mavsdk import System
from mavsdk.offboard import Attitude, AttitudeRate, ActuatorControlGroup, ActuatorControl, VelocityBodyYawspeed
from mavsdk.action_server import *
from enum import Enum

async def set_motors(values, drone: System):
    await drone.action.set_actuator(1, values[0])
    await drone.action.set_actuator(2, values[1])
    await drone.action.set_actuator(3, values[2])
    await drone.action.set_actuator(4, values[3])

async def run():

    drone = System()
    await drone.connect(system_address="serial:///dev/serial0:921600")

    print("Waiting for drone to connect...")
    async for state in drone.core.connection_state():
        if state.is_connected:
            print("-- Connected to drone!")
            break

    ## EITHER USE THIS BLOCK
    # print("--Start of manual control!")
    # await drone.manual_control.set_manual_control_input(0.0, 0.0, 0.0, 0.0)
    # await drone.manual_control.start_altitude_control()
    # await drone.action.arm()
    # await asyncio.sleep(2)
    # val = -1.0
    # for _ in range(100):
    #         print(f"Val: {val}")
    #         await drone.action.set_actuator(1, 0.5)
    #         # await drone.manual_control.set_manual_control_input(val, val, 0.0, val)
    #         val += 2/100
    #         await asyncio.sleep(0.05)
    # await drone.action.kill()
    # print("--End of manual control!")

    ## OR THIS BLOCK
    print("--Start of offboard control!")
    # await drone.offboard.set_attitude(Attitude(roll_deg=0.0, pitch_deg=0.0, yaw_deg=0.0, thrust_value=0.0))
    # await drone.offboard.start()
    #motor_values = ActuatorControl([ActuatorControlGroup([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), 
    #                                ActuatorControlGroup([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])])
    # await drone.offboard.set_actuator_control(motor_values)
    await drone.offboard.set_attitude_rate(AttitudeRate(roll_deg_s=0.0, pitch_deg_s=0.0, yaw_deg_s=0.0, thrust_value=0.0))
    await drone.offboard.start()
    await drone.action.arm()
    val = 0.0
    for t in range(200):
        # await drone.offboard.set_actuator_control(motor_values)
        # await drone.offboard.set_velocity_body(VelocityBodyYawspeed(0.0, 0.0, 0.0, 0.0))
        print(f"Value {val}")
        # await drone.offboard.set_attitude(Attitude(roll_deg=val, pitch_deg=0.0, yaw_deg=0.0, thrust_value=0.0))
        await drone.offboard.set_attitude_rate(AttitudeRate(roll_deg_s=0.0, pitch_deg_s=0.0, yaw_deg_s=0.0, thrust_value=val))
        await asyncio.sleep(0.05)
        val += 1/200
    await drone.action.kill()
    print("--End of offboard control!")

    print("-- The end!")

if __name__ == "__main__":
    # Run the asyncio loop
    asyncio.run(run())
julianoes commented 6 months ago

What software is running on this? ArduSub or something?