Sending MAVProxy messages from a python program?

It is possible to provide python command line, in running a python script
Rpi of Blue ROV2, to

  1. To capture image and video from camera connected to Rpi
  2. To rotate camera servo motor, possiblly through manual mavlink command
    or any other command

Hi @sjpanwar. It is possible to do all of these things that you describe, but we cannot write your programs for you.

Please read more about ardusub, mavlink, pymavlink and mavproxy. All of the information that you need is online. If you have more specific questions or need to know where to find certain information, we can help you. There is also a developer chat that you are welcome to join.

Here are some examples with pymavlink: Pymavlink · GitBook

how to use mavutil.mavfile.set_servo to change the servo PWM??

Take a look in @jacob link.

There you can find in examples that mavlink messages can be used with mavutil, "MAVLink Messages"_send or command_long_send.
If you want to send a set_servo message, mavlink message MAV_CMD_DO_SET_SERVO, you’ll have to use:

mavutil.set_servo(channel, pwm)
#or
master.mav.command_long_send(
    master.target_system, master.target_component,
    mavutil.mavlink.MAV_CMD_DO_SET_SERVO, 0,
    channel, pwm, 0, 0, 0, 0, 0) or:

The _send suffix only exist in messages that doesn’t have the MAV_CMD_DO_ prefix.

Note that you are only allowed to control the PWM output of channels that are not assigned any function. (You cannot control the motor outputs individually).

Hi,

This seems like the right thread for this question.

I have python code working fine to connect to mavlink. I can read data and command the pwm to drive the motors.

My question is am I able to use this command with the BlueROV?

MAV_CMD_CONDITION_YAW

I’ve tried to implement it and nothing happens. Motors are armed.

Thanks!

MT

This command is only used during missions in auto mode. If you want it to yaw, you will have to send MANUAL_CONTROL and give it some r.

Ah. I’m going untethered and I want it to lock onto headings at various times. I wrote a simple P controller to listen for yaw in the mavlink messages and then used the pwm to simulate joystick, but this seems a little clunky.

Sounds like I can put it in auto mode and then try?

Check the MAV_CMD_CONDITION_YAW for that, in python should be something like:

master.mav.command_long_send(
    master.target_system,
    master.target_component,
    mavutil.mavlink.MAV_CMD_CONDITION_YAW,
    0,
    90, 5, 1, 0, 0, 0, 0)

Besides that, the auto mode will only work with a connected position system.

Ah, so I tried that command, but it seems like there’s two reasons it didn’t work?

  1. I wasn’t in auto mode
  2. I didn’t have a connected positioning system.

Does that sound right?

I don’t have a positioning system underwater, but for what I need, I only need to control Yaw underwater. Is there some way to fake positioning system so that I can use the yaw built into pixhawk?

Yes, this python code will only work with guided and auto modes.
You’ll need to send a manual control to perform what you need, probably in stabilize mode.

Check our pymavlink examples, that @jwalser posted here.

To help you, it’s possible to get the yaw and the yaw velocity with the message ATTITUDE.

yaw_msg = master.recv_match(type='ATTITUDE', blocking=True).to_dict()
print(yaw_msg['yaw'], yaw_msg['yawspeed'])

About your controller, try to add a small integrator to help you with the error.

This can make your ROV to go away, because it’ll think that the position is correct all the time.

Great, thanks!

What are the ways I can get the Blue ROV into Auto or Guided mode?

For example, I saw a Mavlink message where I could tell the vehicle its position. Is that one of the ways that might work?

But, from what you said below about my ROV going away…

  • It seems that in auto mode, it will try to hold position and even if I’m only trying to control heading, it will still drift since it’s not getting a position update.

Correct?

I wonder if I keep telling the ROV that it’s current position is it’s actual position, so that it doesn’t think it’s drifting?

My problem with the manual mode is that the update rate of receiving messages to control on seemed sporadic / slow.

I guess I could always add a compass to the RPi, but that takes functionality away from the Pixhawk, which doesn’t seem like the best option.

The only way possible is if the ROV have a GPS fix, take a look here about the modes that need a GPS fix, some do not exist in ArduSub, like Guided_NoGPS.

Yes, you can try to send a GPS message like this one:

    master.mav.gps_raw_int_send(
        0,                     # time_usec                 : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
        3,                     # fix_type                  : See the GPS_FIX_TYPE enum. (uint8_t)
        lat,                   # lat                       : Latitude (WGS84), in degrees * 1E7 (int32_t)
        lon,                   # lon                       : Longitude (WGS84), in degrees * 1E7 (int32_t)
        0,                     # alt                       : Altitude (AMSL, NOT WGS84), in meters * 1000 (positive for up). Note that virtually all GPS modules provide the AMSL altitude in addition to the WGS84 altitude. (int32_t)
        0,                     # eph                       : GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX (uint16_t)
        0,                     # epv                       : GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX (uint16_t)
        0,                     # vel                       : GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX (uint16_t)
        0,                     # cog                       : Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX (uint16_t)
        6                      # satellites_visible        : Number of satellites visible. If unknown, set to 255 (uint8_t)
    )

@jwalser have more experience with this, maybe he can figure out a way to use a “fake GPS” without problems.

You can control the frequency, like described here.

If your goal is to hold a specific heading, the solution is not auto mode. You should try stabilize mode or depth hold modes, the autopilot runs it’s own pid controller to maintain the heading in those stabilized modes. Your program can give yaw input until the heading is where you want it, then just set the input to zero (the input is the desired rate of rotation), and the autopilot will take care of holding its current heading.

Thanks Jacob. Yeah that’s what I was hoping was to have the autopilot do it’s own PID over me doing it on the RPi.

So do you have the Mavlink command handy to do that?

There’s no mavlink command, the autopilot will just hold it’s current heading when the input rate is zero. Use the MANUAL_CONTROL command to emulate pilot input.

I see, so I would still have to make my own PID to control that. If I’m setting a rate and not a heading, then it will probably oscillate without some sort of PID to slow the yaw rate down as it gets close to my desired heading value.

So in case it helps, my needed mission is to have the ROV underwater, untethered, doing the example mission below.

  • Hold depth at 50 feet
  • 0 degrees for 60 seconds.
  • 90 degrees for 60 seconds.
  • 180 degrees for 60 seconds.
  • 270 degrees for 60 seconds.
  • Surface
  • Move to a new GPS location at surface
  • Repeat above

I have already untethered it, installed WiFi, and have done PID control on RPi to command the MANUAL_CONTROL Mavlink messaged based on the master.recv_match to lock onto a heading

The MANUAL_CONTROL just seems kind of clunky and I’d like to have all PID reside within the Autopilot and just send it desired depth and desired heading (aka yaw) commands over having to read the messages and compute my own PID to command the MANUAL_CONTROL.

Also, thanks, this forum is very helpful.

Hi,

I tried to send GPS using the below, but it still says no GPS fix and won’t going into AUTO mode. Any thoughts?

master.mav.gps_raw_int_send(
0, # time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t)
3, # fix_type : See the GPS_FIX_TYPE enum. (uint8_t)
lat, # lat : Latitude (WGS84), in degrees * 1E7 (int32_t)
lon, # lon : Longitude (WGS84), in degrees * 1E7 (int32_t)
0, # alt : Altitude (AMSL, NOT WGS84), in meters * 1000 (positive for up). Note that virtually all GPS modules provide the AMSL altitude in addition to the WGS84 altitude. (int32_t)
0, # eph : GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX (uint16_t)
0, # epv : GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX (uint16_t)
0, # vel : GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX (uint16_t)
0, # cog : Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0…359.99 degrees. If unknown, set to: UINT16_MAX (uint16_t)
6 # satellites_visible : Number of satellites visible. If unknown, set to 255 (uint8_t)
)