def __init__(self, socket_map, mav_iface, send_interval, dispatcher):
Bridge.__init__(self, socket_map, mav_iface, send_interval)
self.dead = False
recv_thread = Thread(target=self._receive)
recv_thread.daemon = True
send_thread = Thread(target=self._send)
send_thread.daemon = True
self.dispatcher = dispatcher
self.auto_mode_flags = MAV_MODE_FLAG_SAFETY_ARMED \
| MAV_MODE_FLAG_MANUAL_INPUT_ENABLED \
| MAV_MODE_FLAG_STABILIZE_ENABLED \
| MAV_MODE_FLAG_GUIDED_ENABLED \
| MAV_MODE_FLAG_AUTO_ENABLED
Bridge.__init__(self, socket_map, mav_iface, send_interval)
self.csb = ControlSensorBits()
self.sensors_present = self.csb.bits([
'GYRO_3D', 'ACC_3D', 'MAG_3D', 'PRESSURE_ABS', 'GPS',
'ANGLE_RATE_CONTROL', 'ATTITUDE_CTRL', 'YAW_CTRL', 'ALTITUDE_CTRL',
'XY_CTRL', 'MOTOR_CTRL'
])
self.sensors_enabled = self.sensors_present
self.sensors_health = self.sensors_present
self._load_reader = LoadReader()
self._power_reader = PowerReader(socket_map['power_mon'])
recv_thread.start()
send_thread.start()
self._load_reader.start()
self._power_reader.start()
self._power_reader.wait_data()
def __init__(self, socket_map, mav_iface, send_interval, dispatcher):
Bridge.__init__(self, socket_map, mav_iface, send_interval)
self.dead = False
recv_thread = Thread(target = self._receive)
recv_thread.daemon = True
send_thread = Thread(target = self._send)
send_thread.daemon = True
self.dispatcher = dispatcher
self.auto_mode_flags = MAV_MODE_FLAG_SAFETY_ARMED \
| MAV_MODE_FLAG_MANUAL_INPUT_ENABLED \
| MAV_MODE_FLAG_STABILIZE_ENABLED \
| MAV_MODE_FLAG_GUIDED_ENABLED \
| MAV_MODE_FLAG_AUTO_ENABLED
Bridge.__init__(self, socket_map, mav_iface, send_interval)
self.csb = ControlSensorBits()
self.sensors_present = self.csb.bits(['GYRO_3D', 'ACC_3D', 'MAG_3D',
'PRESSURE_ABS', 'GPS', 'ANGLE_RATE_CONTROL', 'ATTITUDE_CTRL',
'YAW_CTRL', 'ALTITUDE_CTRL', 'XY_CTRL', 'MOTOR_CTRL'])
self.sensors_enabled = self.sensors_present
self.sensors_health = self.sensors_present
self._load_reader = LoadReader()
self._power_reader = PowerReader(socket_map['power_mon'])
recv_thread.start()
send_thread.start()
self._load_reader.start()
self._power_reader.start()
self._power_reader.wait_data()
class CoreBridge(Bridge):
def __init__(self, socket_map, mav_iface, send_interval, dispatcher):
Bridge.__init__(self, socket_map, mav_iface, send_interval)
self.dead = False
recv_thread = Thread(target = self._receive)
recv_thread.daemon = True
send_thread = Thread(target = self._send)
send_thread.daemon = True
self.dispatcher = dispatcher
self.auto_mode_flags = MAV_MODE_FLAG_SAFETY_ARMED \
| MAV_MODE_FLAG_MANUAL_INPUT_ENABLED \
| MAV_MODE_FLAG_STABILIZE_ENABLED \
| MAV_MODE_FLAG_GUIDED_ENABLED \
| MAV_MODE_FLAG_AUTO_ENABLED
Bridge.__init__(self, socket_map, mav_iface, send_interval)
self.csb = ControlSensorBits()
self.sensors_present = self.csb.bits(['GYRO_3D', 'ACC_3D', 'MAG_3D',
'PRESSURE_ABS', 'GPS', 'ANGLE_RATE_CONTROL', 'ATTITUDE_CTRL',
'YAW_CTRL', 'ALTITUDE_CTRL', 'XY_CTRL', 'MOTOR_CTRL'])
self.sensors_enabled = self.sensors_present
self.sensors_health = self.sensors_present
self._load_reader = LoadReader()
self._power_reader = PowerReader(socket_map['power_mon'])
recv_thread.start()
send_thread.start()
self._load_reader.start()
self._power_reader.start()
self._power_reader.wait_data()
def _dead(self):
self.dead = True
def _receive(self):
mon = MonData()
socket = self.socket_map['core_mon']
last_read = time.time()
while True:
if not self.dead:
timer = Timer(1.0, self._dead)
timer.start()
str = socket.recv()
mon.ParseFromString(str)
self.mon = mon
if not self.dead:
timer.cancel()
self.dead = False
def _send(self):
while True:
time.sleep(self.send_interval)
try:
mon = self.mon
self.mav_iface.send_local_position(mon.x, mon.y, -mon.z,
mon.x_speed, mon.y_speed, -mon.z_speed)
self.mav_iface.send_attitude(mon.roll, mon.pitch, mon.yaw,
mon.roll_speed, mon.pitch_speed, mon.yaw_speed)
ground_speed = sqrt(mon.x_speed ** 2 + mon.y_speed ** 2)
airspeed = 0.0 # TODO: fix me
throttle = 0.5 # todo: fix me
self.mav_iface.mavio.mav.vfr_hud_send(airspeed, ground_speed,
mon.yaw, throttle, -mon.z, -mon.z_speed)
voltage = self._power_reader.power.voltage
current_battery = 0
battery_remaining = -1
drop_rate_comm = int(10000.0 * self.dispatcher.loss_rate)
errors_comm = 0
errors_count1 = 0
errors_count2 = 0
errors_count3 = 0
errors_count4 = 0
if self.dead:
sensors_enabled = 0
self.auto_mode_flags &= ~(MAV_MODE_FLAG_STABILIZE_ENABLED \
| MAV_MODE_FLAG_GUIDED_ENABLED \
| MAV_MODE_FLAG_AUTO_ENABLED)
else:
sensors_enabled = self.sensors_enabled
self.auto_mode_flags |= (MAV_MODE_FLAG_STABILIZE_ENABLED \
| MAV_MODE_FLAG_GUIDED_ENABLED \
| MAV_MODE_FLAG_AUTO_ENABLED)
self.mav_iface.mavio.mav.heartbeat_send(MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, self.auto_mode_flags, 0, MAV_STATE_ACTIVE)
self.mav_iface.mavio.mav.sys_status_send(self.sensors_present, sensors_enabled, 0,
self._load_reader.load * 10, voltage * 1000, current_battery, battery_remaining, drop_rate_comm,
errors_comm, errors_count1, errors_count2, errors_count3, errors_count4)
except Exception, e:
print str(e)
class CoreBridge(Bridge):
def __init__(self, socket_map, mav_iface, send_interval, dispatcher):
Bridge.__init__(self, socket_map, mav_iface, send_interval)
self.dead = False
recv_thread = Thread(target=self._receive)
recv_thread.daemon = True
send_thread = Thread(target=self._send)
send_thread.daemon = True
self.dispatcher = dispatcher
self.auto_mode_flags = MAV_MODE_FLAG_SAFETY_ARMED \
| MAV_MODE_FLAG_MANUAL_INPUT_ENABLED \
| MAV_MODE_FLAG_STABILIZE_ENABLED \
| MAV_MODE_FLAG_GUIDED_ENABLED \
| MAV_MODE_FLAG_AUTO_ENABLED
Bridge.__init__(self, socket_map, mav_iface, send_interval)
self.csb = ControlSensorBits()
self.sensors_present = self.csb.bits([
'GYRO_3D', 'ACC_3D', 'MAG_3D', 'PRESSURE_ABS', 'GPS',
'ANGLE_RATE_CONTROL', 'ATTITUDE_CTRL', 'YAW_CTRL', 'ALTITUDE_CTRL',
'XY_CTRL', 'MOTOR_CTRL'
])
self.sensors_enabled = self.sensors_present
self.sensors_health = self.sensors_present
self._load_reader = LoadReader()
self._power_reader = PowerReader(socket_map['power_mon'])
recv_thread.start()
send_thread.start()
self._load_reader.start()
self._power_reader.start()
self._power_reader.wait_data()
def _dead(self):
self.dead = True
def _receive(self):
mon = MonData()
socket = self.socket_map['core_mon']
last_read = time.time()
while True:
if not self.dead:
timer = Timer(1.0, self._dead)
timer.start()
str = socket.recv()
mon.ParseFromString(str)
self.mon = mon
if not self.dead:
timer.cancel()
self.dead = False
def _send(self):
while True:
time.sleep(self.send_interval)
try:
mon = self.mon
self.mav_iface.send_local_position(mon.x, mon.y, -mon.z,
mon.x_speed, mon.y_speed,
-mon.z_speed)
self.mav_iface.send_attitude(mon.roll, mon.pitch, mon.yaw,
mon.roll_speed, mon.pitch_speed,
mon.yaw_speed)
ground_speed = sqrt(mon.x_speed**2 + mon.y_speed**2)
airspeed = 0.0 # TODO: fix me
throttle = 0.5 # todo: fix me
self.mav_iface.mavio.mav.vfr_hud_send(airspeed, ground_speed,
mon.yaw, throttle,
-mon.z, -mon.z_speed)
voltage = self._power_reader.power.voltage
current_battery = 0
battery_remaining = -1
drop_rate_comm = int(10000.0 * self.dispatcher.loss_rate)
errors_comm = 0
errors_count1 = 0
errors_count2 = 0
errors_count3 = 0
errors_count4 = 0
if self.dead:
sensors_enabled = 0
self.auto_mode_flags &= ~(MAV_MODE_FLAG_STABILIZE_ENABLED \
| MAV_MODE_FLAG_GUIDED_ENABLED \
| MAV_MODE_FLAG_AUTO_ENABLED)
else:
sensors_enabled = self.sensors_enabled
self.auto_mode_flags |= (MAV_MODE_FLAG_STABILIZE_ENABLED \
| MAV_MODE_FLAG_GUIDED_ENABLED \
| MAV_MODE_FLAG_AUTO_ENABLED)
self.mav_iface.mavio.mav.heartbeat_send(
MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
self.auto_mode_flags, 0, MAV_STATE_ACTIVE)
self.mav_iface.mavio.mav.sys_status_send(
self.sensors_present, sensors_enabled, 0,
self._load_reader.load * 10, voltage * 1000,
current_battery, battery_remaining, drop_rate_comm,
errors_comm, errors_count1, errors_count2, errors_count3,
errors_count4)
except Exception, e:
print str(e)
