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)