Newbie PX4 question

[neilyoung]

Hi,

I'm a complete newbie to PX4 gazebo. At least i managed to start the simulator and I'm able to takeoff and land, but only in pxsh.

I'm able to connect to the drone using gomavlib, but I'm too stupid to bring it up in the air.

Anybody having a three liner for instant success?

TIA

[neilyoung]

This is my script. After many attempts.

package main

import (
	"fmt"
	"log"
	"time"

	"github.com/bluenviron/gomavlib/v3"
	"github.com/bluenviron/gomavlib/v3/pkg/dialects/common"
)

func main() {
	node, err := gomavlib.NewNode(gomavlib.NodeConf{
		Endpoints: []gomavlib.EndpointConf{
			gomavlib.EndpointUDPClient{Address: "127.0.0.1:18570"},
		},
		Dialect:     common.Dialect,
		OutVersion:  gomavlib.V2,
		OutSystemID: 10,
	})
	if err != nil {
		log.Fatalf("Error creating MAVLink node: %v", err)
	}
	defer node.Close()
	fmt.Println("Connected to PX4 simulator on port 18570.")

	// Start listening for telemetry and feedback
	go listenForStatusText(node)
	go listenForCommandAck(node)
	go listenForPosition(node)

	// Command sequence
	setHomePosition(node)
	disablePreflightChecks(node)
	armDrone(node)
	setFlightMode(node, 4)       // AUTO.TAKEOFF
	sendTakeoffCommand(node, 20) // Target altitude: 20 meters

	select {} // Keep the program running to listen for updates
}

// Reset the home position to the current location
func setHomePosition(node *gomavlib.Node) {
	fmt.Println("Resetting home position...")
	err := node.WriteMessageAll(&common.MessageCommandLong{
		TargetSystem:    1,
		TargetComponent: 1,
		Command:         common.MAV_CMD_DO_SET_HOME,
		Param1:          1, // Set home to current position
	})
	if err != nil {
		log.Fatalf("Failed to set home position: %v", err)
	}
	fmt.Println("Home position reset.")
	time.Sleep(2 * time.Second)
}

// Disable preflight checks
func disablePreflightChecks(node *gomavlib.Node) {
	fmt.Println("Disabling preflight checks...")
	err := node.WriteMessageAll(&common.MessageCommandLong{
		TargetSystem:    1,
		TargetComponent: 1,
		Command:         common.MAV_CMD_PREFLIGHT_STORAGE,
		Param1:          0, // Save current parameters
	})
	if err != nil {
		log.Fatalf("Failed to disable preflight checks: %v", err)
	}
	fmt.Println("Preflight checks disabled.")
	time.Sleep(2 * time.Second)
}

// Arm the drone
func armDrone(node *gomavlib.Node) {
	fmt.Println("Arming the drone...")
	err := node.WriteMessageAll(&common.MessageCommandLong{
		TargetSystem:    1,
		TargetComponent: 1,
		Command:         common.MAV_CMD_COMPONENT_ARM_DISARM,
		Param1:          1, // Arm the drone
		Param2:          21196, // Magic number for PX4
	})
	if err != nil {
		log.Fatalf("Failed to arm the drone: %v", err)
	}
	fmt.Println("Drone armed.")
	time.Sleep(2 * time.Second)
}

func setFlightMode(node *gomavlib.Node, customMode uint32) {
	fmt.Println("Setting flight mode...")
	err := node.WriteMessageAll(&common.MessageSetMode{
		TargetSystem: 1,
		BaseMode:     common.MAV_MODE(common.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED), // Cast properly
		CustomMode:   customMode, // 4 = AUTO.TAKEOFF
	})
	if err != nil {
		log.Fatalf("Failed to set flight mode: %v", err)
	}
	fmt.Println("Flight mode set.")
	time.Sleep(2 * time.Second)
}

// Send the takeoff command
func sendTakeoffCommand(node *gomavlib.Node, altitude float32) {
	fmt.Printf("Sending takeoff command to %.2f meters...\n", altitude)
	err := node.WriteMessageAll(&common.MessageCommandLong{
		TargetSystem:    1,
		TargetComponent: 1,
		Command:         common.MAV_CMD_NAV_TAKEOFF,
		Param7:          altitude, // Target altitude for takeoff
	})
	if err != nil {
		log.Fatalf("Failed to send takeoff command: %v", err)
	}
	fmt.Println("Takeoff command sent.")
	time.Sleep(5 * time.Second)
}

// Listener for STATUSTEXT messages
func listenForStatusText(node *gomavlib.Node) {
	for evt := range node.Events() {
		if frm, ok := evt.(*gomavlib.EventFrame); ok {
			if status, ok := frm.Message().(*common.MessageStatustext); ok {
				fmt.Printf("STATUSTEXT: Severity=%s, Text=%s\n", status.Severity.String(), status.Text)
			}
		}
	}
}

// Listener for COMMAND_ACK messages
func listenForCommandAck(node *gomavlib.Node) {
	for evt := range node.Events() {
		if frm, ok := evt.(*gomavlib.EventFrame); ok {
			if ack, ok := frm.Message().(*common.MessageCommandAck); ok {
				fmt.Printf("COMMAND_ACK: Command=%s, Result=%s\n", ack.Command.String(), ack.Result.String())
			}
		}
	}
}

// Listener for position updates
func listenForPosition(node *gomavlib.Node) {
	for evt := range node.Events() {
		if frm, ok := evt.(*gomavlib.EventFrame); ok {
			if position, ok := frm.Message().(*common.MessageGlobalPositionInt); ok {
				altitude := float32(position.RelativeAlt) / 1000.0 // Convert to meters
				fmt.Printf("Current Altitude: %.2f meters\n", altitude)
			}
		}
	}
}

Either the simulator says: WARN [navigator] Already higher than takeoff altitude and disarms immediately, or the reported altitude raises in negative direction, w/o showing the drone doing anything...

But nothing useful happens so far.

[aler9]

Hello, take off commands depend on the specific simulator or hardware.

My advice is to place mavp2p between the simulator and a controller that works, for instance QGroundControl. Start mavp2p with the --print flag:

./mavp2p --print tcps:0.0.0.0:5900 tcpc:simulator-ip:5600 ...

Make sure that both QGroundControl and the simulator are connected to mavp2p. Then, perform the take off sequence with QGroundControl. In the console, mavp2p will print the exact commands corresponding to each QGroundControl action that you are performing.