FCU: Cuav 7X Pro
PX Version: Currently release/1.13 I’ve also tried v1.15.2
Logs:
- 1.15.2: https://review.px4.io/plot_app?log=2fba61d8-7b92-4460-bf8e-e5312c5d889e
- release/1.13: https://review.px4.io/plot_app?log=e909debd-2cd2-4470-9e21-29eea7e530a9
I’m trying to use an onboard companion computer to control the flight path of my quad-copter drone in mission mode. The synopsis is thus, I arm the drone, takeoff using the AUTO.TAKEOFF mode, once the desired height is reached the system enters AUTO.LOITER automatically, enables object avoidance, publishes the desired waypoint and then sets the flight mode from AUTO.LOITER (hold) to AUTO.MISSION however the flight controller rejects this mode change and thus the drone just hovers staying in AUTO.LOITER. The interesting thing is that this system works in both SITL (Software in the Loop) and HITL (Hardware in the Loop) modes, which make me feel like its a PX4 issue however I am unsure what it could be as I have tested seemingly everything i can think of for the past 2 weeks. I can’t run QGC while the drone is in flight as my wireless link fails to interface with the Cuav drone and I’m also currently using all the uart ports for other systems (GPS,Depth Sensor,Companion Computer, WIFI)
I would also like to state that in the past it was sort of working however PX4 was actually ignoring my entire object avoidance node, not publishing the trajectory on /mavros/trajectory/desired and just following a direct path to the goal despite COM_OBS_AVOID being enabled. (this was on px4 V1.15.2) if anyone has any ideas about this also, it would be much appreciated.
def set_goal(self, goal:list, command:CommandCode=CommandCode.NAV_WAYPOINT):
"""Sets the goal for the drone to move to in Mission mode http://docs.ros.org/en/lunar/api/mavros_msgs/html/msg/CommandCode.html"""
if not self.current_state.armed:
rospy.logerr("Drone is not armed")
return False
if(self.clear_goal() == False):
return False
if(self.set_avoidance(True)==False):
return False
goal_msg = PoseStamped()
goal_msg.header.frame_id = "map"
goal_msg.header.stamp = rospy.Time.now()
goal_msg.pose.orientation.w = 1
goal_msg.pose.position.x = goal[0]
goal_msg.pose.position.y = goal[1]
goal_msg.pose.position.z = goal[2]
self.current_goal = goal_msg # update the current goal
service_message = self.create_waypoint_service_msg([goal_msg], [command]) # maybe creating the message here will reduce the delay between setting the mode and then publishing the misison, which will cause it to not fail.
# self.set_mode("AUTO.MISSION") #! the mode MUST be set before the goal is pushed
response = self._srv_mission(self.create_waypoint_service_msg([goal_msg,goal_msg], [command, CommandCode.NAV_LOITER_UNLIM])) #! added a loiter mission goal at the goal
response:WaypointPushResponse = self._srv_mission(service_message)
self.set_mode("AUTO.MISSION")
if not response.success:
rospy.logerr("Failed to push mission")
return False
rospy.loginfo(f"Goal set to {goal[0], goal[1], goal[2]}, wp_id: {response.wp_transfered}")
rospy.sleep(0.5) # sleep to allow the state to update
return True
def set_mode(self, mode_name:str):
rospy.wait_for_service('/mavros/set_mode')
"""Sets the mode of the drone http://wiki.ros.org/mavros/CustomModes"""
response = self._srv_set_mode(SetModeRequest(base_mode=0, custom_mode=mode_name))
if not response.mode_sent:
rospy.logerr(f"Failed to set mode to {mode_name}")
return False
rospy.sleep(1) # sleep to allow the state to uppdate, reduce if necessary but ensure that it works
if self.current_state.mode != mode_name:
rospy.logerr(f"mode {self.current_state.mode} failed to update to {mode_name}")
return False
rospy.loginfo(f"Mode set to {mode_name}")
return True
A synopsis of how the function runs is that avoidance is enabled first, then a mission is published to mavros via /mavros/mission/push followed by setting the mode via the /mavros/set_mode service.
Update: I have found a way to move one of my UART devices to one of the AUX out lines, which means I can now run QGC on the newly freed UART port while the drone is in flight, which should hopefully glean more information.