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.

Sign up for GitHub

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

Updated code from pidrone_pkg and moved to python3 #8

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Updated code from pidrone_pkg and moved to python3 #8

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
868f6f7
Updated code from pidrone_pkg and to python3
Tuxliri Apr 30, 2023
af5bb02
Updated shebangs to Python 3
Tuxliri Apr 30, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
187 changes: 93 additions & 94 deletions student_flight_controller_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,12 +1,19 @@
#!/usr/bin/env python
#!/usr/bin/env python3

import tf
import traceback
import sys
import yaml

import rospy
import rospkg
import signal
import numpy as np

print("tf import")
import tf
print("done tf import")


import command_values as cmds
from sensor_msgs.msg import Imu
from h2rMultiWii import MultiWii
Expand Down Expand Up @@ -39,7 +46,9 @@ class FlightController(object):

def __init__(self):
# Connect to the flight controller board
print("getboard")
self.board = self.getBoard()
print("done")
# stores the current and previous modes
self.curr_mode = 'DISARMED' #initialize as disarmed
self.prev_mode = 'DISARMED' #initialize as disarmed
Expand All @@ -51,42 +60,44 @@ def __init__(self):
# store the time for angular velocity calculations
self.time = rospy.Time.now()


# Initialize the Imu Message
############################
# TODO: create an Imu message with some initial values.
# In a different method, the message will be updated and
# published (you will do this in another TODO).
# Hint: investigate the Header message type, the Imu
# message type, and rospy.Time.
self.imu_message = ???
self.imu_message.header = ???
self.imu_message = ...
self.imu_message.header = ...
self.imu_message.header.frame_id = 'Body'
self.imu_message.header.stamp = ???
self.imu_message.header.stamp = ...

# Initialize the Battery Message
################################
# TODO: create a Battery message with some initial values.
# In a different method, the message will be updated and
# published (you will do this in another TODO). Hint: read
# the Battery message file in the pidrone_pkg msg folder.
self.battery_message = ???
self.battery_message.??? = None
self.battery_message.??? = None
self.battery_message = ...
self.battery_message.CHANGE_FIELD_HERE = None
self.battery_message.CHANGE_FIELD_HERE = None

# Adjust this based on how low the battery should discharge
self.minimum_voltage = 4.5

# Accelerometer parameters
##########################
print("loading")
rospack = rospkg.RosPack()
path = rospack.get_path('pidrone_pkg')
with open("%s/params/multiwii.yaml" % path) as f:
means = yaml.load(f)
print("done")
self.accRawToMss = 9.8 / means["az"]
self.accZeroX = means["ax"] * self.accRawToMss
self.accZeroY = means["ay"] * self.accRawToMss
self.accZeroZ = means["az"] * self.accRawToMss
self.accZeroX=means["ax"] * self.accRawToMss
self.accZeroY=means["ay"] * self.accRawToMss
self.accZeroZ=means["az"] * self.accRawToMss
pass


# ROS subscriber callback methods:
Expand All @@ -113,23 +124,23 @@ def update_imu_message(self):
Compute the ROS IMU message by reading data from the board.
"""

# Extract roll, pitch, heading
# extract roll, pitch, heading
self.board.getData(MultiWii.ATTITUDE)
# Extract lin_acc_x, lin_acc_y, lin_acc_z
# extract lin_acc_x, lin_acc_y, lin_acc_z
self.board.getData(MultiWii.RAW_IMU)

# TODO: calculate roll, pitch, and yaw in radians.
# Hint: roll is rotation about which axis? What about pitch?
# Hint: what data is in self.board.attitude? What about
# self.board.rawIMU? Is the relevant data in degrees or radians?
# Hint: yaw is sometimes referred to as "heading".
roll = ???
pitch = ???
heading = ???
roll = ...
pitch = ...
heading = ...
# Note that at pitch angles near 90 degrees, the roll angle reading can
# fluctuate a lot

# Transform heading (yaw) to standard math conventions, which
# transform heading (similar to yaw) to standard math conventions, which
# means angles are in radians and positive rotation is CCW
heading = (-heading) % (2 * np.pi)

Expand All @@ -141,7 +152,7 @@ def update_imu_message(self):
# and vice-versa.
# Hint: investigate tf.transformations.euler_from_quaternion.
previous_quaternion = self.imu_message.orientation
previous_roll, previous_pitch, previous_heading = ???
previous_roll, previous_pitch, previous_heading = ...

# Although quaternion_from_euler takes a heading in range [0, 2pi),
# euler_from_quaternion returns a heading in range [0, pi] or [0, -pi).
Expand All @@ -152,89 +163,70 @@ def update_imu_message(self):
# Hint: recall that Euler angles can be transformed to quaternions,
# and vice-versa.
# Hint: investigate tf.transformations.quaternion_from_euler.
quaternion = ???
quaternion = ...

# TODO: extract the raw linear accelerations from the flight controller.
# Hint: what data is in self.board.attitude? What about
# self.board.rawIMU?
raw_acc_x = ???
raw_acc_y = ???
raw_acc_z = ???
raw_acc_x = ...
raw_acc_y = ...
raw_acc_z = ...

# Turn the raw linear accelerations into real accelerations
lin_acc_x = raw_acc_x * self.accRawToMss - self.accZeroX
lin_acc_y = raw_acc_y * self.accRawToMss - self.accZeroY
lin_acc_z = raw_acc_z * self.accRawToMss - self.accZeroZ


# TODO: extract the raw angular velicities from the flight controller.
raw_ang_vel_x = ???
raw_ang_vel_y = ???
raw_ang_vel_z = ???

# Calculate the rotational rates


rospack = rospkg.RosPack()
path = rospack.get_path('pidrone_pkg')
with open("%s/params/multiwii.yaml" % path) as f:
means = yaml.load(f)
self.XGyroRawToRs = 0.0010569610567923715 # rad/s/raw
self.YGyroRawToRs = 0.0010533920049110032 # rad/s/raw
self.ZGyroRawToRs = 0.0010644278634753999 # rad/s/raw
self.gyroZeroX = means["gx"] * self.XGyroRawToRs # raw * rad/s/raw = rad/s
self.gyroZeroY = means["gy"] * self.YGyroRawToRs # raw * rad/s/raw = rad/s
self.gyroZeroZ = means["gz"] * self.ZGyroRawToRs # raw * rad/s/raw = rad/s
ang_vel_x = raw_ang_vel_x * self.XGyroRawToRs - self.gyroZeroX
ang_vel_y = raw_ang_vel_y * self.YGyroRawToRs - self.gyroZeroY
ang_vel_z = raw_ang_vel_z * self.ZGyroRawToRs - self.gyroZeroZ

# Rotate the IMU frame to align with our convention for the drone's body
# frame. IMU accelerometer: x is forward, y is left, z is up. We want: x
# is right, y is forward, z is up.
# ACC
# frame. IMU: x is forward, y is left, z is up. We want: x is right,
# y is forward, z is up.
lin_acc_x_drone_body = -lin_acc_y
lin_acc_y_drone_body = lin_acc_x
lin_acc_z_drone_body = lin_acc_z
# GYRO
ang_vel_x_drone_body = ang_vel_x
ang_vel_y_drone_body = -ang_vel_y
ang_vel_z_drone_body = ang_vel_z

# Account for gravity's affect on linear acceleration values when roll
# and pitch are nonzero. When the drone is pitched at 90 degrees, for
# example, the z acceleration reads out as -9.8 m/s^2. This makes sense,
# since our calibration variable, accZeroZ, zeros the body-frame z-axis
# acceleration to 0 when the drone is level. However, when it's pitched
# as the IMU, when powered up / when the calibration script is called,
# zeros the body-frame z-axis acceleration to 0, but when it's pitched
# 90 degrees, the body-frame z-axis is perpendicular to the force of
# gravity the IMU reads -9.8 m/s^2 along the z-axis.
# gravity, so, as if the drone were in free-fall (which was roughly
# confirmed experimentally), the IMU reads -9.8 m/s^2 along the z-axis.
g = 9.8
lin_acc_x_drone_body = lin_acc_x_drone_body + g * np.sin(roll) * np.cos(pitch)
lin_acc_y_drone_body = lin_acc_y_drone_body + g * np.cos(roll) * (-np.sin(pitch))
lin_acc_z_drone_body = lin_acc_z_drone_body + g * (1 - np.cos(roll) * np.cos(pitch))
lin_acc_x_drone_body = lin_acc_x_drone_body + g*np.sin(roll)*np.cos(pitch)
lin_acc_y_drone_body = lin_acc_y_drone_body + g*np.cos(roll)*(-np.sin(pitch))
lin_acc_z_drone_body = lin_acc_z_drone_body + g*(1 - np.cos(roll)*np.cos(pitch))

# calculate the angular velocities of roll, pitch, and yaw in rad/s
time = rospy.Time.now()
dt = ...
dr = ...
dp = ...
dh = ...
angvx = self.near_zero(dr / dt)
angvy = self.near_zero(dp / dt)
angvz = self.near_zero(dh / dt)
self.time = time

# TODO: Update the imu_message header stamp.
self.imu_message.header.stamp = ???
self.imu_message.header.stamp = ...

# TODO: update the IMU message orientation
# Hint: is the orientation a set of Euler angles or a quaternion?
self.imu_message.orientation.x = ???
self.imu_message.orientation.y = ???
self.imu_message.orientation.z = ???
self.imu_message.orientation.w = ???
self.imu_message.orientation.x = ...
self.imu_message.orientation.y = ...
self.imu_message.orientation.z = ...
self.imu_message.orientation.w = ...

# TODO: update the IMU message angular velocities.
self.imu_message.??? = ang_vel_x_drone_body
self.imu_message.??? = ang_vel_y_drone_body
self.imu_message.??? = ang_vel_z_drone_body
self.imu_message.CHANGE_FIELD_HERE = ang_vel_x_drone_body
self.imu_message.CHANGE_FIELD_HERE = ang_vel_y_drone_body
self.imu_message.CHANGE_FIELD_HERE = ang_vel_z_drone_body

# TODO: update the IMU message linear accelerations.
self.imu_message.linear_acceleration.x = ???
self.imu_message.linear_acceleration.y = ???
self.imu_message.linear_acceleration.z = ???
self.imu_message.linear_acceleration.x = ...
self.imu_message.linear_acceleration.y = ...
self.imu_message.linear_acceleration.z = ...


def update_battery_message(self):
Expand All @@ -246,8 +238,8 @@ def update_battery_message(self):


# TODO: Update Battery message:
self.battery_message.vbat = ??? * 0.10
self.battery_message.amperage = ???
self.battery_message.vbat = ... * 0.10
self.battery_message.amperage = ...



Expand All @@ -270,12 +262,12 @@ def getBoard(self):
try:
board = MultiWii('/dev/ttyUSB0')
except SerialException as e:
print("usb0 failed: " + str(e))
print(("usb0 failed: " + str(e)))
try:
board = MultiWii('/dev/ttyUSB1')
except SerialException:
print '\nCannot connect to the flight controller board.'
print 'The USB is unplugged. Please check connection.'
print('\nCannot connect to the flight controller board.')
print('The USB is unplugged. Please check connection.')
raise
sys.exit()
return board
Expand All @@ -285,7 +277,7 @@ def send_cmd(self):
self.board.sendCMD(8, MultiWii.SET_RAW_RC, self.command)
self.board.receiveDataPacket()
if (self.command != self.last_command):
print 'command sent:', self.command
print(('command sent:', self.command))
self.last_command = self.command

def near_zero(self, n):
Expand All @@ -294,12 +286,12 @@ def near_zero(self, n):

def ctrl_c_handler(self, signal, frame):
""" Disarm the drone and quits the flight controller node """
print "\nCaught ctrl-c! About to Disarm!"
print("\nCaught ctrl-c! About to Disarm!")
self.board.sendCMD(8, MultiWii.SET_RAW_RC, cmds.disarm_cmd)
self.board.receiveDataPacket()
rospy.sleep(1)
self.modepub.publish('DISARMED')
print "Successfully Disarmed"
print("Successfully Disarmed")
sys.exit()

# Heartbeat Callbacks: These update the last time that data was received
Expand Down Expand Up @@ -342,6 +334,10 @@ def shouldIDisarm(self):
print('\nSafety Failure: not receiving data from the IR sensor.')
print('Check the infrared node\n')
disarm = True

if self.range > 0.5:
print(('\nSafety Failure: too high: ' + str(self.range)))
disarm = True
if curr_time - self.heartbeat_state_estimator > rospy.Duration.from_sec(1):
print('\nSafety Failure: not receiving a state estimate.')
print('Check the state_estimator node\n')
Expand All @@ -354,12 +350,14 @@ def main():
# ROS Setup
###########
node_name = os.path.splitext(os.path.basename(__file__))[0]
print("init")
rospy.init_node(node_name)

print("done")
# create the FlightController object
fc = FlightController()
curr_time = rospy.Time.now()
fc.heartbeat_infrared = curr_time
fc.range = None
fc.heartbeat_web_interface= curr_time
fc.heartbeat_pid_controller = curr_time
fc.heartbeat_flight_controller = curr_time
Expand All @@ -370,10 +368,10 @@ def main():
imupub = rospy.Publisher('/pidrone/imu', Imu, queue_size=1, tcp_nodelay=False)
batpub = rospy.Publisher('/pidrone/battery', Battery, queue_size=1, tcp_nodelay=False)
fc.modepub = rospy.Publisher('/pidrone/mode', Mode, queue_size=1, tcp_nodelay=False)
print 'Publishing:'
print '/pidrone/imu'
print '/pidrone/mode'
print '/pidrone/battery'
print('Publishing:')
print('/pidrone/imu')
print('/pidrone/mode')
print('/pidrone/battery')

# Subscribers
############
Expand All @@ -393,10 +391,10 @@ def main():
while not rospy.is_shutdown():
# if the current mode is anything other than disarmed
# preform as safety check
if fc.curr_mode != 'DISARMED':
# Break the loop if a safety check has failed
if fc.shouldIDisarm():
break
if fc.shouldIDisarm():
print(("mode", fc.curr_mode))
break

# update and publish flight controller readings
fc.update_battery_message()
Expand All @@ -415,13 +413,14 @@ def main():
r.sleep()

except SerialException:
print '\nCannot connect to the flight controller board.'
print 'The USB is unplugged. Please check connection.'
except:
print 'there was an internal error'
print('\nCannot connect to the flight controller board.')
print('The USB is unplugged. Please check connection.')
except Exception as e:
print(('there was an internal error', e))
print((traceback.format_exc()))
finally:
print 'Shutdown received'
print 'Sending DISARM command'
print('Shutdown received')
print('Sending DISARM command')
fc.board.sendCMD(8, MultiWii.SET_RAW_RC, cmds.disarm_cmd)
fc.board.receiveDataPacket()

Expand Down
Loading