def set_attitude_target(self, tolerance=10):
"""Test setting of attitude target in guided mode."""
self.change_mode("GUIDED")
# self.set_parameter("STALL_PREVENTION", 0)
state_roll_over = "roll-over"
state_stabilize_roll = "stabilize-roll"
state_hold = "hold"
state_roll_back = "roll-back"
state_done = "done"
tstart = self.get_sim_time()
try:
state = state_roll_over
while state != state_done:
m = self.mav.recv_match(type='ATTITUDE',
blocking=True,
timeout=0.1)
now = self.get_sim_time_cached()
if now - tstart > 20:
raise AutoTestTimeoutException("Manuevers not completed")
if m is None:
continue
r = math.degrees(m.roll)
if state == state_roll_over:
target_roll_degrees = 60
if abs(r - target_roll_degrees) < tolerance:
state = state_stabilize_roll
stabilize_start = now
elif state == state_stabilize_roll:
# just give it a little time to sort it self out
if now - stabilize_start > 2:
state = state_hold
hold_start = now
elif state == state_hold:
target_roll_degrees = 60
if now - hold_start > tolerance:
state = state_roll_back
if abs(r - target_roll_degrees) > tolerance:
raise NotAchievedException("Failed to hold attitude")
elif state == state_roll_back:
target_roll_degrees = 0
if abs(r - target_roll_degrees) < tolerance:
state = state_done
else:
raise ValueError("Unknown state %s" % str(state))
m_nav = self.mav.messages['NAV_CONTROLLER_OUTPUT']
self.progress("%s Roll: %f desired=%f set=%f" %
(state, r, m_nav.nav_roll, target_roll_degrees))
time_boot_millis = 0 # FIXME
target_system = 1 # FIXME
target_component = 1 # FIXME
type_mask = 0b10000001 ^ 0xFF # FIXME
# attitude in radians:
q = quaternion.Quaternion(
[math.radians(target_roll_degrees), 0, 0])
roll_rate_radians = 0.5
pitch_rate_radians = 0
yaw_rate_radians = 0
thrust = 1.0
self.mav.mav.set_attitude_target_send(
time_boot_millis, target_system, target_component,
type_mask, q, roll_rate_radians, pitch_rate_radians,
yaw_rate_radians, thrust)
except Exception as e:
self.mavproxy.send('mode FBWA\n')
self.wait_mode('FBWA')
self.set_rc(3, 1700)
raise e
# back to FBWA
self.mavproxy.send('mode FBWA\n')
self.wait_mode('FBWA')
self.set_rc(3, 1700)
self.wait_level_flight()
def set_attitude_target(self):
"""Test setting of attitude target in guided mode."""
# mode guided:
self.mavproxy.send('mode GUIDED\n')
self.wait_mode('GUIDED')
target_roll_degrees = 70
state_roll_over = "roll-over"
state_stabilize_roll = "stabilize-roll"
state_hold = "hold"
state_roll_back = "roll-back"
state_done = "done"
tstart = self.get_sim_time()
try:
state = state_roll_over
while state != state_done:
if self.get_sim_time() - tstart > 20:
raise AutoTestTimeoutException("Manuevers not completed")
m = self.mav.recv_match(type='ATTITUDE',
blocking=True,
timeout=0.1)
if m is None:
continue
r = math.degrees(m.roll)
if state == state_roll_over:
target_roll_degrees = 70
if abs(r - target_roll_degrees) < 10:
state = state_stabilize_roll
stabilize_start = self.get_sim_time()
elif state == state_stabilize_roll:
# just give it a little time to sort it self out
if self.get_sim_time() - stabilize_start > 2:
state = state_hold
hold_start = self.get_sim_time()
elif state == state_hold:
target_roll_degrees = 70
if self.get_sim_time() - hold_start > 10:
state = state_roll_back
if abs(r - target_roll_degrees) > 10:
raise NotAchievedException("Failed to hold attitude")
elif state == state_roll_back:
target_roll_degrees = 0
if abs(r - target_roll_degrees) < 10:
state = state_done
else:
raise ValueError("Unknown state %s" % str(state))
self.progress("%s Roll: %f desired=%f" %
(state, r, target_roll_degrees))
time_boot_millis = 0 # FIXME
target_system = 1 # FIXME
target_component = 1 # FIXME
type_mask = 0b10000001 ^ 0xFF # FIXME
# attitude in radians:
q = quaternion.Quaternion([math.radians(target_roll_degrees),
0,
0])
roll_rate_radians = 0.5
pitch_rate_radians = 0
yaw_rate_radians = 0
thrust = 1.0
self.mav.mav.set_attitude_target_send(time_boot_millis,
target_system,
target_component,
type_mask,
q,
roll_rate_radians,
pitch_rate_radians,
yaw_rate_radians,
thrust)
except Exception as e:
self.mavproxy.send('mode FBWA\n')
self.wait_mode('FBWA')
self.set_rc(3, 1700)
raise e
# back to FBWA
self.mavproxy.send('mode FBWA\n')
self.wait_mode('FBWA')
self.set_rc(3, 1700)
self.wait_level_flight()
def main_loop():
'''main processing loop'''
if not mpstate.status.setup_mode and not opts.nowait:
for master in mpstate.mav_master:
send_heartbeat(master)
if master.linknum == 0:
print("Waiting for heartbeat from %s" % master.address)
master.wait_heartbeat()
set_stream_rates()
while True:
if mpstate is None or mpstate.status.exit:
return
while not mpstate.input_queue.empty():
line = mpstate.input_queue.get()
mpstate.input_count += 1
cmds = line.split(';')
if len(cmds) == 1 and cmds[0] == "":
mpstate.empty_input_count += 1
for c in cmds:
process_stdin(c)
for master in mpstate.mav_master:
if master.fd is None:
if master.port.inWaiting() > 0:
process_master(master)
periodic_tasks()
rin = []
for master in mpstate.mav_master:
if master.fd is not None and not master.portdead:
rin.append(master.fd)
for m in mpstate.mav_outputs:
rin.append(m.fd)
for sysid in mpstate.sysid_outputs:
m = mpstate.sysid_outputs[sysid]
rin.append(m.fd)
if rin == []:
time.sleep(0.0001)
continue
for fd in mpstate.select_extra:
rin.append(fd)
try:
(rin, win, xin) = select.select(rin, [], [], mpstate.settings.select_timeout)
except select.error:
continue
if mpstate is None:
return
for fd in rin:
if mpstate is None:
return
for master in mpstate.mav_master:
if fd == master.fd:
process_master(master)
if mpstate is None:
return
continue
for m in mpstate.mav_outputs:
if fd == m.fd:
process_mavlink(m)
if mpstate is None:
return
continue
for sysid in mpstate.sysid_outputs:
m = mpstate.sysid_outputs[sysid]
if fd == m.fd:
process_mavlink(m)
if mpstate is None:
return
continue
# this allow modules to register their own file descriptors
# for the main select loop
if fd in mpstate.select_extra:
try:
# call the registered read function
(fn, args) = mpstate.select_extra[fd]
fn(args)
except Exception as msg:
if mpstate.settings.moddebug == 1:
print(msg)
# on an exception, remove it from the select list
mpstate.select_extra.pop(fd)
########################## Jorge Cano CODE ##########################
Rollvalue = mpstate.status.msgs['ATTITUDE'].roll #rad
Pitchvalue = mpstate.status.msgs['ATTITUDE'].pitch #rad
Yawvalue = mpstate.status.msgs['ATTITUDE'].yaw #rad
# ESTIMATED: fused GPS and accelerometers
Latvalue = math.radians((mpstate.status.msgs['GLOBAL_POSITION_INT'].lat)/1E7) #rad
Lonvalue = math.radians((mpstate.status.msgs['GLOBAL_POSITION_INT'].lon)/1E7) #rad
Altvalue = (mpstate.status.msgs['GLOBAL_POSITION_INT'].alt)/1000 #meters
PH_quat = quaternion.Quaternion([Rollvalue, Pitchvalue, Yawvalue])
PH_xyz = spherical2cartesian(Latvalue, Lonvalue, Altvalue)
#print (PH_quat)
#print (PH_xyz)
PHx = PH_xyz[0]
PHy = PH_xyz[1]
PHz = PH_xyz[2]
PHh = 1 #Altvalue
PHq0 = PH_quat.__getitem__(0)
PHq1 = PH_quat.__getitem__(1)
PHq2 = PH_quat.__getitem__(2)
PHq3 = PH_quat.__getitem__(3)
PH_Pose3D.setPose3DData(PHx,PHy,PHz,PHh,PHq0,PHq1,PHq2,PHq3)