Problem With Changing Flight Mode GUIDED to AUTO

AstroMix · March 18, 2021, 3:35pm

Task:
I have the task to make a drone autonomous, the idea is to make a drone move to waypoints using AUTO mode with Python, rospy, ArduPilot SITL, Mavros, Mavlink and Gazebo 9. ROS melodic is used. All in Linux 18.04. No map.

Code:

github.com

A-Kotecha/vigilant-enigmas/blob/main/GNC_node_with_redundancy_Auto.py

#!/usr/bin/env python

# GNC Node - communicates with the controller and the Task Allocation node

import rospy
import numpy as np
from std_msgs.msg import String
from std_msgs.msg import Float64 #get Heading
from sensor_msgs.msg import NavSatFix #get GPS coordinates
from sensor_msgs.msg import Imu #Get IMU
from sensor_msgs.msg import BatteryState #get Battery
#from gdp_centralised.msg import AgentInfo #System Architecture
from mavros_msgs.msg import State #FCU state
from mavros_msgs.msg import Waypoint # Waypoint format
from mavros_msgs.msg import WaypointReached # Waypoint reaching
from mavros_msgs.srv import CommandTOL # Take Off and Land
from mavros_msgs.srv import CommandHome #set Home Location
from mavros_msgs.srv import CommandBool #arming/disarming
from mavros_msgs.srv import SetMode # set flight mode
from mavros_msgs.srv import WaypointClear #Clear waypoint table

This file has been truncated. show original

Problem:
The problem is when I use guided mode to take off, I cannot change the mode with Python from GUIDED to AUTO.
When I use MAV_CMD_NAV_TAKEOFF ([22]) to takeoff the drone, when loading waypoints , the first waypoint get SITL_START_LOCATION overwritten by from: https://github.com/ardupilot/ardupilot/blob/master/Tools/autotest/arducopter.py#L3382 meaning the drone doesnt takeoff as the first command is a 16 and not a 22 command. When takeoff using FBW in ArduPilot SITL terminal, the UAV flies to somewhere fly which I’m assuming is the SITL_START_LOCATION.
The specific failure in MAVLink I get when trying to change the mode from GUIDED to AUTO is “APM: Flight mode change failed Got MAVLink msg: COMMAND ACK{Command:11, result:3}”

Thank you in Advanced, any help would be much appreciated!