I am working on my master thesis, which will be focused on autonomous exploration using a quadcopter. In terms of the drone setup our current plan is to include the following hardware and software:
PixHawk 4 FCU running the latest version of ArduCopter
Here+ GPS module (already present, can be removed)
Aaeon Up Board with an Intel RealSense D435i RGB-D camera (for SLAM) and USB WiFi module
A distributed ROS Kinetic network, to offload the Aaeon Up.
MAVROS will send attitude commands
Our problem is that we are unable to get the drone to take off using attitude commands (which include thrust). Right now, we have a gamepad setup to essentially emulate a regular RC-controller. These inputs are converted to a <mavros_msgs::AttitudeTarget> ROS message, where we specify the orientation using a quaternion (orientation.w/x/y/z, as well as the desired yaw rate (body_rate.z) and thrust. The published ROS topic name is mavros/setpoint_raw/attitude, which should be subscribed by MAVROS.
(Just now, I saw that setpoint_raw mentions PX4, so maybe we simply need to switch over to setpoint_attitude…?)
We can successfully switch to the GUIDED_NOGPS mode and arm the drone using the gamepad, so MAVROS is able to pick up on (at least some) our commands. We suspect that we are missing a step. Perhaps a take-off command is required? How would this work when our positional data will be supplied by a high-level SLAM position estimate, instead of a GPS and distance sensor? Maybe the built-in barometer and IMU from the PixHawk already does this under the hood?
Note that we will be conducting testing in an empty section of a parking garage, so ideally we only want to start hovering as close to the floor as possible before sending out attitude messages…
Do you see the topic /mavros/setpoint_raw/attitude being published in mavros ? If not, can you provide the code you use to publish in it.
As @amilcarlucas said, if you use ArduCopter, I saw a problem from mavros (the command thrust was not taking into account) that I changed days ago https://github.com/mavlink/mavros/pull/1178
You can install from source, or just change as I did in the pull request.
We currently have the following version of MAVROS installed, which is the latest available using “sudo apt-get install ros-kinetic-mavros ros-kinetic-mavros-extras”: 0.28.0-0xenial-20190113-132234-0800.
I see that your issue will be added to the 0.30 release, so I suppose we will have to make this change manually. We did so by simply changing the relevant .yaml-file in /opt/ros/kinetic/share/mavros/launch using sudo.
We tested the drone again after doing this. Indeed the /mavros/setpoint_raw/attitude message is shown when we run “rostopic list” and “rostopic echo /mavros/setpoint_raw/attitude” on the on-board computer. The addition we made does not appear to have made a difference.
Also - we failed to mention this previously (silly us), but our motors keep beeping and the GPS module blinks steadily with a yellow light. According to http://ardupilot.org/copter/docs/common-leds-pixhawk.html, this should mean that the battery fail safe is activated. However, we are measuring 24.5V on our 6S battery…
We will post our MissionPlanner parameters soon.
EDIT: We are supplying thrust in the range [0,100], quaternion attitude, and yaw rate as [-1,1]. Perhaps thrust should only be 0-1, but even so, a higher value should just revert to maximum thrust, right?
Can you check in the topic /mavros/state and /diagnostic that you are indeed in GUIDED_NOGPS mode ?
Thrust should be between 0-1, because GUIDED_NOGPS altitude works like ALT_HOLD.
So if Thrust < 0.5 => Go down
If Thrust > 0.5 => Go up
If Thrust == 0.5 => Stabilize at the same altitude.
If the motors steel beeping, it means to me that the ESC is power up (from the battery), but they receive no information from the Pixhawk. Check the wiring to the Pixhawk, and check that the “safety switch button” is constant red : http://ardupilot.org/copter/docs/common-pixhawk-overview.html
Ah, this new information seems to have solved our problem! I’m fairly sure we have pressed the button before (but forgot about it now), so maybe it was the thrust value range (maybe invalid values are ignored?) AND the bug fix. Anyway, thanks a lot for the help! We’ve been struggling with this for quite some time.
My Copter are going takeoff and land smoothly in GUIDED_NOGPS. But copter did not stay target altitude. I used thrush = 0.5 when Reached target altitude.
@borhanreo,
Can you describe a little more your situation. Did you succeed to make it fly ? If so, do you have the logs of it ?
Besides, can you provide more information like :
What kind of rangefinder do you use ?
Do you use mavros to communicate ? If so, is the topics about GUIDED_NOGPS are publishing datas ?
etc etc
def arm_and_takeoff_nogps(aTargetAltitude):
DEFAULT_TAKEOFF_THRUST = 0.6
SMOOTH_TAKEOFF_THRUST = 0.5
print("Basic pre-arm checks")
while not vehicle.is_armable:
print(" Waiting for vehicle to initialise...")
time.sleep(1)
print("Arming motors")
vehicle.mode = VehicleMode("GUIDED_NOGPS")
vehicle.armed = True
while not vehicle.armed:
print(" Waiting for arming...")
vehicle.armed = True
time.sleep(1)
print("Taking off!")
thrust = DEFAULT_TAKEOFF_THRUST
while True:
#current_altitude = vehicle.location.global_relative_frame.alt
#if sonar_alt<4.00:
current_altitude = sonar_alt
print(" Altitude: %f Desired: %f" % (current_altitude, aTargetAltitude))
if current_altitude >= aTargetAltitude * 0.95:
print("Reached target altitude")
break
elif current_altitude >= aTargetAltitude * 0.6:
thrust = SMOOTH_TAKEOFF_THRUST
set_attitude(thrust=thrust)
time.sleep(0.2)
def set_attitude(roll_angle=0.0, pitch_angle=0.0, yaw_rate=0.0, thrust=0.5, duration=0):
# Thrust > 0.5: Ascend
# Thrust == 0.5: Hold the altitude
# Thrust < 0.5: Descend
msg = vehicle.message_factory.set_attitude_target_encode(
0, # time_boot_ms
1, # Target system
1, # Target component
0b00000000, # Type mask: bit 1 is LSB
to_quaternion(roll_angle, pitch_angle), # Quaternion
0, # Body roll rate in radian
0, # Body pitch rate in radian
math.radians(yaw_rate), # Body yaw rate in radian
thrust # Thrust
)
vehicle.send_mavlink(msg)
start = time.time()
while time.time() - start < duration:
vehicle.send_mavlink(msg)
time.sleep(0.1)
def to_quaternion(roll=0.0, pitch=0.0, yaw=0.0):
t0 = math.cos(math.radians(yaw * 0.5))
t1 = math.sin(math.radians(yaw * 0.5))
t2 = math.cos(math.radians(roll * 0.5))
t3 = math.sin(math.radians(roll * 0.5))
t4 = math.cos(math.radians(pitch * 0.5))
t5 = math.sin(math.radians(pitch * 0.5))
w = t0 * t2 * t4 + t1 * t3 * t5
x = t0 * t3 * t4 - t1 * t2 * t5
y = t0 * t2 * t5 + t1 * t3 * t4
z = t1 * t2 * t4 - t0 * t3 * t5
return [w, x, y, z]
def drone_goto(target_alt,position_hold_duration):
arm_and_takeoff_nogps(1.0)
print("Hold position for 3 seconds")
set_attitude(duration=10)
print("Move forward")
# set_attitude(pitch_angle=-5, thrust=0.5, duration=3.21)
# print("Move backward")
# set_attitude(pitch_angle=5, thrust=0.5, duration=3)
vehicle.mode = VehicleMode("LAND")
time.sleep(1)
if sonar_alt<0.18:
vehicle.mode = VehicleMode("STABILIZE")
**When Reached target altitude then thrust set 0.5. but drone can not stay exact altitude. drone is going through up as you said
if Thrust < 0.5 => Go down
If Thrust > 0.5 => Go up
If Thrust == 0.5 => Stabilize at the same altitude.
As I see in your code, it should stay at the altitude. It can comes from the sensor, the barometer and the rangefinder can be noisy in certain environment.
As you can see, the desired altitude (CTUN.DAlt) is kept when you go to GUIDED_NOGPS, and the altitude is around it. Besides, you can see the throttle with the topic “CTUN.ThO”, to see the respond to the motors to keep the same altitude.
For me, all is OK.
But for the altitude, there is a lot of noise in your rangefinder :
As you can see in the second graph, the RangeFinder.Distance is not coherent with the altitude provided by the barometer (BARO.Alt). So firstly, you should know where it does come from (wiring, settings etc.)
Then, for me, your system works. It keeps the same altitude as what you asked.
Maybe it could be better that you takeoff manually, keep an certain altitude, then switch to GUIDED_NOGPS mode. You will see that the altitude is kept.
In your experience, it is hard to see it, because first you go to the highest altitude possible (with the cable), and then you asks to keep this one. So it is normal that the drone set throttle to reach this high altitude.
Hi @KiloNovemberDelta: i am sorry for late. i was busy another approach. Now i configured my maxbotix range finder with pixhawk. Get graph is that ok???
@KiloNovemberDelta
So now i get value as default primary altitude from lidar(i use maxbotix). current_altitude = vehicle.location.global_relative_frame.alt
current_altitude get from lidar or baro???
I’m fairly sure we have pressed the button before (but forgot about it now), so maybe it was the thrust value range (maybe invalid values are ignored?) AND the bug fix. Anyway, thanks a lot for the help! We’ve been struggling with this for quite some time.
