Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Executing commands on Pixhawk with ArduPilot via MAVROS2 #1863

Open
OliverZanone opened this issue May 31, 2023 · 4 comments
Open

Executing commands on Pixhawk with ArduPilot via MAVROS2 #1863

OliverZanone opened this issue May 31, 2023 · 4 comments

Comments

@OliverZanone
Copy link

OliverZanone commented May 31, 2023

Issue details

Finding information for MAVROS2 has been pretty frustrating as relevant information seems scarce.
As much as I have struggled to get some of this working, I would be more than happy to write up some tutorials after all of this.
The best source of information I've found came from this post here.

From this, I have been attempting to manually deploy the commands to a Pixhawk FMU via a rpi companion computer using a combination of services and especially commandLong, i.e. for arming:
ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: True}"
For setting mode to guided:
ros2 service call /mavros/cmd/command mavros_msgs/srv/CommandLong "{command: 176, param1: 1, param2: 4}"
I later realized you must also follow your firmware's documentation as well, and the supported parameters found here for ardupilot, thus the values for param1 and param2.
I can confirm the mode is being set in mission planner flight modes tab.

Since I was able to get those working, I have tried working through other commands found in the mavlink docs and then crossreferencing those with ardupilot documentation.
However, all other commands I've tested have failed.

For example, taking off:
ros2 service call /mavros/cmd/command mavros_msgs/srv/CommandLong "{command: 22, param7: 5}"
With broadcast set to false, nothing happens/no output. Setting broadcast to true shows "Unexpected command 22, result 4" I have all of the prearm safety checks unchecked, other than the hardware button found on my gps unit.

Trying the built in mavros service for taking off:
ros2 service call /mavros/cmd/takeoff mavros_msgs/srv/CommandTOL "{altitude: 5}"
this gives success=false result=4, which according to mavlink docs, 4 means good command but failed to execute, as in the prior method.

My goal is to implement all of the guided commands found here programatically. Some of these fall under the message category, not the command category on mavlink docs, which commandLong I assume is only for the command category.

I assume some of these commands are built into the other mavros services, but as I cannot find any documentation on how to use them, it has really been a game of trial and error on using just the commandLong.

If someone could point me in the right direction on either finding documentation on some of the mavros services or figuring out how to implement some of these commands and messages found in mavlink/ardupilot documentation that would be greatly appreciated.

MAVROS version and platform

Mavros2: 2.4.0-1jammy.20230429.052850
ROS2: Humble
Ubuntu: 22.04.2 LTS (in docker container running on raspberry pi bullseye)

Autopilot type and version

[ x ] ArduPilot
[ ] PX4

Version: V4.0.6

Node logs

ros2 run mavros mavros_node --ros-args --param fcu_url:=/dev/serial0:921600
[INFO] [1685560039.392028775] [mavros_node]: Starting mavros_node container
[INFO] [1685560039.393001800] [mavros_node]: FCU URL: /dev/serial0:921600
[INFO] [1685560039.393076130] [mavros_node]: GCS URL:
[INFO] [1685560039.393122974] [mavros_node]: UAS Prefix: /uas1
[INFO] [1685560039.393175300] [mavros_node]: Starting mavros router node
[INFO] [1685560039.444330369] [mavros_router]: Built-in SIMD instructions: ARM NEON
[INFO] [1685560039.444455785] [mavros_router]: Built-in MAVLink package version: 2022.12.30
[INFO] [1685560039.444505481] [mavros_router]: Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all cubepilot development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta
[INFO] [1685560039.444946280] [mavros_router]: MAVROS Router started
[INFO] [1685560039.445124022] [mavros_router]: Requested to add endpoint: type: 0, url: /dev/serial0:921600
[INFO] [1685560039.445441961] [mavros_router]: Endpoint link[1000] created
[INFO] [1685560039.447051659] [mavros_router]: link[1000] opened successfully
[INFO] [1685560039.447727656] [mavros_router]: Requested to add endpoint: type: 2, url: /uas1
[INFO] [1685560039.447875133] [mavros_router]: Endpoint link[1001] created
[INFO] [1685560039.473473311] [mavros_router]: link[1001] opened successfully
[INFO] [1685560039.473701175] [mavros_node]: Starting mavros uas node
[INFO] [1685560040.019120526] [mavros]: UAS Executor started, threads: 4
[INFO] [1685560040.105053786] [mavros_router]: link[1000] detected remote address 1.1
[INFO] [1685560040.197345404] [mavros]: Plugin actuator_control created
[INFO] [1685560040.197727226] [mavros]: Plugin actuator_control initialized
[INFO] [1685560040.219377512] [mavros]: Plugin altitude created
[INFO] [1685560040.219758593] [mavros]: Plugin altitude initialized
[INFO] [1685560040.253212990] [mavros]: Plugin command created
[INFO] [1685560040.253993712] [mavros]: Plugin command initialized
[INFO] [1685560040.299947462] [mavros]: Plugin ftp created
[INFO] [1685560040.300486039] [mavros]: Plugin ftp initialized
[INFO] [1685560040.327320672] [mavros_router]: link[1000] detected remote address 255.190
[INFO] [1685560040.345631653] [mavros]: Plugin geofence created
[INFO] [1685560040.346170361] [mavros]: Plugin geofence initialized
[INFO] [1685560040.425000698] [mavros]: Plugin global_position created
[INFO] [1685560040.425283926] [mavros]: Plugin global_position initialized
[INFO] [1685560040.455821830] [mavros]: Plugin home_position created
[INFO] [1685560040.456012982] [mavros]: Plugin home_position initialized
[INFO] [1685560040.495909216] [mavros]: Plugin imu created
[INFO] [1685560040.496534646] [mavros]: Plugin imu initialized
[INFO] [1685560040.537043993] [mavros]: Plugin local_position created
[INFO] [1685560040.537356800] [mavros]: Plugin local_position initialized
[INFO] [1685560040.569562524] [mavros]: Plugin manual_control created
[INFO] [1685560040.569862903] [mavros]: Plugin manual_control initialized
[INFO] [1685560040.605607066] [mavros]: Plugin nav_controller_output created
[INFO] [1685560040.605913872] [mavros]: Plugin nav_controller_output initialized
[INFO] [1685560040.648115284] [mavros]: Plugin param created
[INFO] [1685560040.648414051] [mavros]: Plugin param initialized
[INFO] [1685560040.689578667] [mavros]: Plugin rallypoint created
[INFO] [1685560040.689938374] [mavros]: Plugin rallypoint initialized
[INFO] [1685560040.734199595] [mavros]: Plugin rc_io created
[INFO] [1685560040.734553374] [mavros]: Plugin rc_io initialized
[INFO] [1685560040.775540119] [mavros]: Plugin setpoint_accel created
[INFO] [1685560040.775831904] [mavros]: Plugin setpoint_accel initialized
[INFO] [1685560040.833126906] [mavros]: Plugin setpoint_attitude created
[INFO] [1685560040.833424858] [mavros]: Plugin setpoint_attitude initialized
[INFO] [1685560040.881690564] [mavros]: Plugin setpoint_position created
[INFO] [1685560040.881970957] [mavros]: Plugin setpoint_position initialized
[INFO] [1685560040.941746970] [mavros]: Plugin setpoint_raw created
[INFO] [1685560040.942177729] [mavros]: Plugin setpoint_raw initialized
[INFO] [1685560041.004217965] [mavros]: Plugin setpoint_trajectory created
[INFO] [1685560041.004457146] [mavros]: Plugin setpoint_trajectory initialized
[INFO] [1685560041.051443287] [mavros]: Plugin setpoint_velocity created
[INFO] [1685560041.052370002] [mavros]: Plugin setpoint_velocity initialized
[INFO] [1685560041.132124661] [mavros]: Plugin sys_status created
[INFO] [1685560041.132521393] [mavros]: Plugin sys_status initialized
[INFO] [1685560041.180333687] [mavros.time]: TM: Timesync mode: MAVLINK
[INFO] [1685560041.204536786] [mavros]: Plugin sys_time created
[INFO] [1685560041.205203427] [mavros]: Plugin sys_time initialized
[INFO] [1685560041.274914536] [mavros]: Plugin waypoint created
[INFO] [1685560041.276013842] [mavros]: Plugin waypoint initialized
[INFO] [1685560041.326329629] [mavros]: Plugin wind_estimation created
[INFO] [1685560041.327015033] [mavros]: Plugin wind_estimation initialized
[INFO] [1685560041.335158996] [mavros]: Built-in SIMD instructions: ARM NEON
[INFO] [1685560041.335393341] [mavros]: Built-in MAVLink package version: 2022.12.30
[INFO] [1685560041.335751065] [mavros]: Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all cubepilot development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta
[INFO] [1685560041.335892521] [mavros]: MAVROS UAS via /uas1 started. MY ID 1.191, TARGET ID 1.1
[INFO] [1685560042.105601993] [mavros_router]: link[1001] detected remote address 1.191
[INFO] [1685560042.106730693] [mavros]: CON: Got HEARTBEAT, connected. FCU: ArduPilot
[INFO] [1685560042.107150892] [mavros.mission]: WP: detected enable_partial_push: 1
[WARN] [1685560043.120856319] [mavros.cmd]: CMD: Unexpected command 520, result 0
[INFO] [1685560043.123189822] [mavros.geofence]: GF: Using MISSION_ITEM_INT
[INFO] [1685560043.123424555] [mavros.rallypoint]: RP: Using MISSION_ITEM_INT
[INFO] [1685560043.123557694] [mavros.mission]: WP: Using MISSION_ITEM_INT
[INFO] [1685560043.123695538] [mavros.sys]: VER: 1.1: Capabilities         0x000000000000fbef
[INFO] [1685560043.123939885] [mavros.sys]: VER: 1.1: Flight software:     040006ff ( 13b6478)
[INFO] [1685560043.125047930] [mavros.sys]: VER: 1.1: Middleware software: 00000000 (        )
[INFO] [1685560043.127954626] [mavros.sys]: VER: 1.1: OS software:         00000000 ( d4fce84)
[INFO] [1685560043.128778482] [mavros.sys]: VER: 1.1: Board hardware:      00000000
[INFO] [1685560043.129227946] [mavros.sys]: VER: 1.1: VID/PID:             0000:0000
[INFO] [1685560043.130315894] [mavros.sys]: VER: 1.1: UID:                 0000000000000000
[INFO] [1685560052.110910799] [mavros.home_position]: HP: requesting home position
[INFO] [1685560052.120636269] [mavros.sys]: FCU: ArduCopter V4.0.6 (13b6478d)
[INFO] [1685560052.122951230] [mavros.sys]: FCU: ChibiOS: d4fce84e
[INFO] [1685560052.123850143] [mavros.sys]: FCU: Pixhawk4 0022002A 3039510A 30323632
[INFO] [1685560052.125447448] [mavros.sys]: FCU: RCOut: PWM:1-16
[INFO] [1685560052.127345237] [mavros.sys]: FCU: Frame: QUAD
[INFO] [1685560057.123559209] [mavros.mission]: WP: item #0  F:0 C: 16 p: 0 0 0 0 x: 0 y: 0 z: 0
[INFO] [1685560057.129077327] [mavros.mission]: WP: item #1  F:3 C: 22 p: 0 0 0 0 x: 0 y: 0 z: 1.2
[INFO] [1685560057.133729861] [mavros.mission]: WP: item #2  F:3 C: 16 p: 0 0 0 0 x: [redacted] z: 1.2
[INFO] [1685560057.141242770] [mavros.mission]: WP: item #3  F:3 C: 19 p: 10 0 1 0 x: 0 y: 0 z: 0
[INFO] [1685560057.145940405] [mavros.mission]: WP: item #4  F:3 C: 16 p: 0 0 0 0 x: [redacted] z: 1.2
[INFO] [1685560057.151173860] [mavros.mission]: WP: item #5  F:3 C: 19 p: 10 0 1 0 x: 0 y: 0 z: 0
[INFO] [1685560057.155935340] [mavros.mission]: WP: item #6  F:0 C: 20 p: 0 0 0 0 x: 0 y: 0 z: 0
[INFO] [1685560057.156227208] [mavros.mission]: WP: mission received
[INFO] [1685560062.119786920] [mavros.rallypoint]: RP: mission received
[INFO] [1685560067.118827158] [mavros.geofence]: GF: mission received
[WARN] [1685560068.315949789] [mavros.param]: PR: Param STAT_RUNTIME (65535/1028): 461229 different index: 802/1028
[INFO] [1685560073.014858062] [mavros.param]: PR: parameters list received

Diagnostics

ros2 topic echo /diagnostics
header:
  stamp:
    sec: 1685565457
    nanosec: 420412182
  frame_id: ''
status:
- level: "\0"
  name: 'mavros_router: MAVROS Router'
  message: ok
  hardware_id: none
  values:
  - key: Endpoints
    value: '2'
  - key: Messages routed
    value: '21358'
  - key: Messages sent
    value: '21358'
  - key: Messages dropped
    value: '0'
- level: "\0"
  name: 'mavros_router: endpoint 1000: /dev/serial0:921600'
  message: ok
  hardware_id: none
  values:
  - key: Received packets
    value: '15349'
  - key: Dropped packets
    value: '0'
  - key: Buffer overruns
    value: '0'
  - key: Parse errors
    value: '0'
  - key: Rx sequence number
    value: '20'
  - key: Tx sequence number
    value: '0'
  - key: Rx total bytes
    value: '340585'
  - key: Tx total bytes
    value: '129587'
  - key: Rx speed
    value: inf
  - key: Tx speed
    value: inf
  - key: Remotes count
    value: '5'
  - key: Remote [0]
    value: '0.0'
  - key: Remote [1]
    value: '1.0'
  - key: Remote [2]
    value: '1.1'
  - key: Remote [3]
    value: '255.0'
  - key: Remote [4]
    value: '255.190'
- level: "\0"
  name: 'mavros_router: endpoint 1001: /uas1'
  message: ok
  hardware_id: none
  values:
  - key: Remotes count
    value: '3'
  - key: Remote [0]
    value: '0.0'
  - key: Remote [1]
    value: '1.0'
  - key: Remote [2]
    value: '1.191'
---
header:
  stamp:
    sec: 1685565457
    nanosec: 485896285
  frame_id: ''
status:
- level: "\0"
  name: 'mavros: MAVROS UAS'
  message: connected
  hardware_id: uas:///uas1
  values: []
- level: "\x02"
  name: 'mavros: GPS'
  message: No satellites
  hardware_id: uas:///uas1
  values:
  - key: Satellites visible
    value: '0'
  - key: Fix type
    value: '0'
  - key: EPH (m)
    value: Unknown
  - key: EPV (m)
    value: Unknown
- level: "\0"
  name: 'mavros: System'
  message: Normal
  hardware_id: uas:///uas1
  values:
  - key: Sensor present
    value: '0x00000000'
  - key: Sensor enabled
    value: '0x00000000'
  - key: Sensor health
    value: '0x00000000'
  - key: CPU Load (%)
    value: '0.0'
  - key: Drop rate (%)
    value: '0.0'
  - key: Errors comm
    value: '0'
  - key: 'Errors count #1'
    value: '0'
  - key: 'Errors count #2'
    value: '0'
  - key: 'Errors count #3'
    value: '0'
  - key: 'Errors count #4'
    value: '0'
- level: "\x02"
  name: 'mavros: Battery'
  message: No data
  hardware_id: uas:///uas1
  values:
  - key: Voltage
    value: '-1.00'
  - key: Current
    value: '0.0'
  - key: Remaining
    value: '0.0'
- level: "\0"
  name: 'mavros: Heartbeat'
  message: Normal
  hardware_id: uas:///uas1
  values:
  - key: Heartbeats since startup
    value: '5422'
  - key: Frequency (Hz)
    value: '1.000019'
  - key: Vehicle type
    value: Quadrotor
  - key: Autopilot type
    value: ArduPilot
  - key: Mode
    value: LOITER
  - key: System status
    value: STANDBY
---
header:
  stamp:
    sec: 1685565458
    nanosec: 420407111
  frame_id: ''
status:
- level: "\0"
  name: 'mavros_router: MAVROS Router'
  message: ok
  hardware_id: none
  values:
  - key: Endpoints
    value: '2'
  - key: Messages routed
    value: '21365'
  - key: Messages sent
    value: '21365'
  - key: Messages dropped
    value: '0'
- level: "\0"
  name: 'mavros_router: endpoint 1000: /dev/serial0:921600'
  message: ok
  hardware_id: none
  values:
  - key: Received packets
    value: '15354'
  - key: Dropped packets
    value: '0'
  - key: Buffer overruns
    value: '0'
  - key: Parse errors
    value: '0'
  - key: Rx sequence number
    value: '25'
  - key: Tx sequence number
    value: '0'
  - key: Rx total bytes
    value: '340693'
  - key: Tx total bytes
    value: '129634'
  - key: Rx speed
    value: inf
  - key: Tx speed
    value: inf
  - key: Remotes count
    value: '5'
  - key: Remote [0]
    value: '0.0'
  - key: Remote [1]
    value: '1.0'
  - key: Remote [2]
    value: '1.1'
  - key: Remote [3]
    value: '255.0'
  - key: Remote [4]
    value: '255.190'
- level: "\0"
  name: 'mavros_router: endpoint 1001: /uas1'
  message: ok
  hardware_id: none
  values:
  - key: Remotes count
    value: '3'
  - key: Remote [0]
    value: '0.0'
  - key: Remote [1]
    value: '1.0'
  - key: Remote [2]
    value: '1.191'
---
header:
  stamp:
    sec: 1685565458
    nanosec: 486012749
  frame_id: ''
status:
- level: "\0"
  name: 'mavros: MAVROS UAS'
  message: connected
  hardware_id: uas:///uas1
  values: []
- level: "\x02"
  name: 'mavros: GPS'
  message: No satellites
  hardware_id: uas:///uas1
  values:
  - key: Satellites visible
    value: '0'
  - key: Fix type
    value: '0'
  - key: EPH (m)
    value: Unknown
  - key: EPV (m)
    value: Unknown
- level: "\0"
  name: 'mavros: System'
  message: Normal
  hardware_id: uas:///uas1
  values:
  - key: Sensor present
    value: '0x00000000'
  - key: Sensor enabled
    value: '0x00000000'
  - key: Sensor health
    value: '0x00000000'
  - key: CPU Load (%)
    value: '0.0'
  - key: Drop rate (%)
    value: '0.0'
  - key: Errors comm
    value: '0'
  - key: 'Errors count #1'
    value: '0'
  - key: 'Errors count #2'
    value: '0'
  - key: 'Errors count #3'
    value: '0'
  - key: 'Errors count #4'
    value: '0'
- level: "\x02"
  name: 'mavros: Battery'
  message: No data
  hardware_id: uas:///uas1
  values:
  - key: Voltage
    value: '-1.00'
  - key: Current
    value: '0.0'
  - key: Remaining
    value: '0.0'
- level: "\0"
  name: 'mavros: Heartbeat'
  message: Normal
  hardware_id: uas:///uas1
  values:
  - key: Heartbeats since startup
    value: '5423'
  - key: Frequency (Hz)
    value: '1.000012'
  - key: Vehicle type
    value: Quadrotor
  - key: Autopilot type
    value: ArduPilot
  - key: Mode
    value: LOITER
  - key: System status
    value: STANDBY
---
header:
  stamp:
    sec: 1685565459
    nanosec: 420396558
  frame_id: ''
status:
- level: "\0"
  name: 'mavros_router: MAVROS Router'
  message: ok
  hardware_id: none
  values:
  - key: Endpoints
    value: '2'
  - key: Messages routed
    value: '21368'
  - key: Messages sent
    value: '21368'
  - key: Messages dropped
    value: '0'
- level: "\0"
  name: 'mavros_router: endpoint 1000: /dev/serial0:921600'
  message: ok
  hardware_id: none
  values:
  - key: Received packets
    value: '15356'
  - key: Dropped packets
    value: '0'
  - key: Buffer overruns
    value: '0'
  - key: Parse errors
    value: '0'
  - key: Rx sequence number
    value: '26'
  - key: Tx sequence number
    value: '0'
  - key: Rx total bytes
    value: '340735'
  - key: Tx total bytes
    value: '129655'
  - key: Rx speed
    value: inf
  - key: Tx speed
    value: inf
  - key: Remotes count
    value: '5'
  - key: Remote [0]
    value: '0.0'
  - key: Remote [1]
    value: '1.0'
  - key: Remote [2]
    value: '1.1'
  - key: Remote [3]
    value: '255.0'
  - key: Remote [4]
    value: '255.190'

Check ID

Router topic: /uas1/mavlink_source, target: 1.1
OK. I got messages from 1.1.
---
Received 50, from 2 addresses
address   list of messages
255.190   0 (HEARTBEAT), 183, 66 (REQUEST_DATA_STREAM), 111 (TIMESYNC)
    1.1   0 (HEARTBEAT), 22 (PARAM_VALUE), 111 (TIMESYNC)
@vooon
Copy link
Member

vooon commented May 31, 2023

Hi,

Check id - ros2 run mavros mav checkid. But i already see ids in router diag. A bit strange that all system stats are zero, and that there is not battery.

https://mavlink.io/en/messages/common.html#MAV_RESULT_FAILED why that happened most likely be sent as STATUSTEXT, which printed with FCU: prefix.

@OliverZanone
Copy link
Author

Hello, I added my check ID to the first post. It seems to be outputting correctly. I actually was able to get the takeoff and land commands to work. However, there is still very limited output on the mavros_node console. Is there a way to make this more verbose, i.e. have it output when the mode changes and when it is performing a command?

My prior issues were as follows: I was indoors with no GPS signal. I was attempting to test the Guided_NoGPS mode, and have it take off in that mode. I am unsure of why this was not working, I am guessing there is a different protocol/command to use for this mode. After moving to an area with GPS and retesting in guided mode, takeoff and landing worked as expected.
For others, the following is for setting mode, arming, taking off, then landing:

ros2 service call /mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: True}"
ros2 service call /mavros/cmd/command mavros_msgs/srv/CommandLong "{command: 176, param1: 1, param2: 4}"
# Can also use this:
# ros2 service call /mavros/set_mode mavros_msgs/srv/SetMode "{base_mode: 0, custom_mode: 4}"
ros2 service call /mavros/cmd/takeoff mavros_msgs/srv/CommandTOL "{altitude: 5}"
ros2 service call /mavros/cmd/land mavros_msgs/srv/CommandTOL

What mavros command would be for SET_POSITION_TARGET_LOCAL_NED?
I see a long list of mavros2 services (using ros2 list services -t), but with no documentation on them, have no idea which would be the correct one.

Would these messages maybe be params that can then be set? Is there something similar to CommandLong but for messages?
Thanks

@vooon
Copy link
Member

vooon commented Jun 2, 2023

Version 2 mostly have same messages and plugins as v1. So you can list topics and see that you're asking about setpoint* plugins.

Actually checkid doesn't looks correct, to few messages coming from 1.1. Should be much more than just a heartbeat.
Maybe you need to check port configuration and also try to request data streams.

@wntun
Copy link

wntun commented Aug 6, 2024

Hello @OliverZanone , I'm using the Cube Blue and cannot even change the flight mode. Could you share the parameters you used for serial?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

3 participants