Hi all,
I ran the code below to control the bluerov2. However, I came across the problem that the pwm send to the thrusters are apparently not correct. I use the pymavlink to send the depth channel with the pwm value of 1800. And the video shows that the thrusters spin at a very low speed, definitely not the speed corresponding to the value of 1800.
I tried the joystick to control the bluerov, and everything worked fine. So I guess if there is something wrong with the raspberry Pi or the Ardusub firmware. I also tried to upgrade the pixhawk and install the new version Raspberry Pi, but it changed nothing. What should I do now? Is there any thing with my code?
I also have a question with regard to the Calibration of Compass and Accelerator. Is the calibration needed every time when I upload the new ardusub firmware and raspberry Pi? Because when I open QGC after the update of the firmware, I found that there are green dots (instead of red) beside the calibration part of Compass and Accelerator, which means the calibration has been finished. However, I think that the former firmware has been erased, how can the calibration be remembered? If so, does that mean the upload is unsuccessful?
# -*- coding: utf-8 -*-
"""
Example of how to use RC_CHANNEL_OVERRIDE messages to force input channels
in Ardupilot. These effectively replace the input channels (from joystick
or radio), NOT the output channels going to thrusters and servos.
"""
import sys
import time
from pymavlink import mavutil
master = mavutil.mavlink_connection('udpin:0.0.0.0:14567')
master.wait_heartbeat()
# Arm armBlueRov2
def arm():
master.mav.command_long_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0,
1, 0, 0, 0, 0, 0, 0)
# Disarm
def disarm():
master.mav.command_long_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
0,
0, 0, 0, 0, 0, 0, 0)
# Create a function to send RC values
# More information about Joystick channels
# here: https://www.ardusub.com/operators-manual/rc-input-and-output.html#rc-inputs
def set_rc_channel_pwm(channel_id, pwm=1500):
""" Set RC channel pwm value
Args:
channel_id (TYPE): Channel ID
pwm (int, optional): Channel pwm value 1100-1900
"""
if channel_id < 1 or channel_id > 8:
print("Channel does not exist.")
return
# Mavlink 2 supports up to 18 channels:
# https://mavlink.io/en/messages/common.html#RC_CHANNELS_OVERRIDE
rc_channel_values = [65535 for _ in range(18)]
for i in range(6):
rc_channel_values[i] = 1500
rc_channel_values[channel_id - 1] = pwm
# print(rc_channel_values)
master.mav.rc_channels_override_send(
master.target_system, # target_system
master.target_component, # target_component
*rc_channel_values) # RC channel list, in microseconds.
def clear_motion(stopped_pwm=1500):
''' Sets all 6 motion direction RC inputs to 'stopped_pwm'. '''
rc_channel_values = [65535 for _ in range(18)]
for i in range(6):
rc_channel_values[i] = stopped_pwm
master.mav.rc_channels_override_send(
master.target_system, # target_system
master.target_component, # target_component
*rc_channel_values) # RC channel list, in microseconds.
# print('clear_motion')
def change_mode(mode='MANUAL'):
"""
:param mode: 'MANUAL' or 'STABILIZE' or 'ALT_HOLD'
"""
if mode not in master.mode_mapping():
print("Unknown mode : {}".format(mode))
print('Try: ', list(master.mode_mapping().keys()))
sys.exit(1)
mode_id = master.mode_mapping()[mode]
master.mav.set_mode_send(
master.target_system,
mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
mode_id)
while True:
# Wait for ACK command
ack_msg = master.recv_match(type='COMMAND_ACK', blocking=True)
ack_msg = ack_msg.to_dict()
# Check if command in the same in `set_mode`
if ack_msg['command'] == mavutil.mavlink.MAVLINK_MSG_ID_SET_MODE:
print('success')
break
def request_message_interval(message_id, frequency_hz):
"""
Request MAVLink message in a desired frequency,
documentation for SET_MESSAGE_INTERVAL:
https://mavlink.io/en/messages/common.html#MAV_CMD_SET_MESSAGE_INTERVAL
Args:
message_id (int): MAVLink message ID
frequency_hz (float): Desired frequency in Hz
"""
master.mav.command_long_send(
master.target_system, master.target_component,
mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, 0,
message_id, # The MAVLink message ID
1e6 / frequency_hz,
# The interval between two messages in microseconds. Set to -1 to disable and 0 to request default rate.
0,
# Target address of message stream (if message has target address fields). 0: Flight-stack default (recommended), 1: address of requestor, 2: broadcast.
0, 0, 0, 0)
if __name__ == "__main__":
print('setting flight mode')
change_mode("MANUAL")
print('Start clearing motion')
clear_motion()
time.sleep(1)
arm()
# wait until arming confirmed (can manually check with master.motors_armed())
print("Waiting for the vehicle to arm")
master.motors_armed_wait()
print('Armed!')
set_rc_channel_pwm(3, 1800)
# while True:
# clear_motion()
# # 1-Pitch 2-Roll 3-Throttle 4-Yaw 5-Forward 6-Lateral
# set_rc_channel_pwm(3, 1800)
Following is the video of ROV performance,
https://www.kapwing.com/videos/61d749a82e4c5501414c15ca