Example #1
0
    def move_based_local_ned(self, vel_x, vel_y, vel_z, duration_ms=500):
        Logger.PrintLog(
            self.LOG_INFO,
            "move_based_local_ned() - Moving based on NED. X:{} m/s, Y:{} m/s, Z:{} m/s."
            .format(str(vel_x), str(vel_y), str(vel_z)))

        if self._drone.telemetry().get_flight_mode() != "GUIDED":
            Logger.PrintLog(
                self.LOG_INFO,
                "move_based_local_ned() - Cannot move based on NED. Drone is not in GUIDED mode."
            )
            return False

        start_timestamp = Time.get_current_timestamp("ms")

        while 1:
            eb_mavutil.Enum.get_method_reference(
                self._drone.mav(), "SET_POSITION_TARGET_LOCAL_NED")(
                    0, self._drone._get_target_system(),
                    self._drone._get_target_component(),
                    eb_mavutil.Enum.get_int_reference("MAV_FRAME_LOCAL_NED"),
                    0b0000111111000111, 0, 0, 0, vel_x, vel_y, vel_z, 0, 0, 0,
                    0, 0)

            if duration_ms <= 0 \
            or Time.get_current_timestamp("ms") - start_timestamp >= duration_ms:
                break

        return True
Example #2
0
        def impl(cls, hold):
            Logger.PrintLog(
                cls.LOG_INFO,
                "hold_global_position() - Handler has started. Lat:{}, Lon:{}, Alt:{}."
                .format(str(hold["current_pos"]["lat"]),
                        str(hold["current_pos"]["lon"]),
                        str(hold["current_pos"]["relative_alt"])))

            start_timestamp = Time.get_current_timestamp("ms")
            update_counter = -1

            while hold["hold"] \
            and   self._drone.telemetry().get_flight_mode() == "GUIDED":
                if hold.get("skip", False): continue
                if hold.get("pos_override", None) is not None:
                    Logger.PrintLog(
                        cls.LOG_INFO,
                        "hold_global_position() - impl() - Position override detected. Current lat: {}, lon:{}, relative alt:{}. Overridden lat: {}, lon: {}, relative alt: {}."
                        .format(str(hold["current_pos"]["lat"]),
                                str(hold["current_pos"]["lon"]),
                                str(hold["current_pos"]["relative_alt"]),
                                str(hold["pos_override"]["lat"]),
                                str(hold["pos_override"]["lon"]),
                                str(hold["pos_override"]["relative_alt"])))

                    hold["current_pos"] = hold["pos_override"]
                    hold["pos_override"] = None

                if floor((Time.get_current_timestamp("ms") - start_timestamp) /
                         update_interval) > update_counter:
                    COMMAND = eb_mavutil.Enum.get_int_reference(
                        "MAV_CMD_NAV_WAYPOINT")

                    eb_mavutil.Enum.get_method_reference(
                        self._drone.mav(),
                        "MISSION_ITEM")(self._drone._get_target_system(),
                                        self._drone._get_target_component(), 0,
                                        eb_mavutil.Enum.get_int_reference(
                                            "MAV_FRAME_GLOBAL_RELATIVE_ALT"),
                                        COMMAND, 2, 0, 0, 0, 0, 0,
                                        hold["current_pos"]["lat"],
                                        hold["current_pos"]["lon"],
                                        hold["current_pos"]["relative_alt"])

                    update_counter += 1

            hold["ended"] = True

            Logger.PrintLog(self.LOG_INFO,
                            "hold_global_position() - Handler has ended.")
Example #3
0
        def wait_until_value(dictionary: dict,
                             key: str,
                             value: any,
                             timeout_ms: int = 10000) -> None:
            if timeout_ms < 0: raise ValueError("Timeout < 0.")

            start_timestamp = Time.get_current_timestamp("ms")

            while timeout_ms == 0 \
            or    Time.get_current_timestamp("ms") - start_timestamp <= timeout_ms:
                if key not in dictionary: continue
                if dictionary[key] == value: return

            raise TimeoutError
Example #4
0
        def impl(cls, hold):
            Logger.PrintLog(
                cls.LOG_INFO,
                "hold_local_position() - Handler has started. X:{}, Y:{}, Z:{}."
                .format(str(hold["current_pos"]["x"]),
                        str(hold["current_pos"]["y"]),
                        str(hold["current_pos"]["z"])))

            start_timestamp = Time.get_current_timestamp("ms")
            update_counter = -1

            while hold["hold"]:
                if hold.get("skip", False): continue
                if hold.get("pos_override", None) is not None:
                    Logger.PrintLog(
                        cls.LOG_INFO,
                        "hold_local_position() - impl() - Position override detected. Current X: {}, Y:{}, Z:{}. Overridden X: {}, Y: {}, Z: {}."
                        .format(str(hold["current_pos"]["x"]),
                                str(hold["current_pos"]["y"]),
                                str(hold["current_pos"]["z"]),
                                str(hold["pos_override"]["x"]),
                                str(hold["pos_override"]["y"]),
                                str(hold["pos_override"]["z"])))

                    hold["current_pos"] = hold["pos_override"]
                    hold["pos_override"] = None

                if floor((Time.get_current_timestamp("ms") - start_timestamp) /
                         update_interval) > update_counter:
                    COMMAND = eb_mavutil.Enum.get_int_reference(
                        "MAV_CMD_NAV_WAYPOINT")

                    eb_mavutil.Enum.get_method_reference(
                        self._drone.mav(),
                        "MISSION_ITEM")(self._drone._get_target_system(),
                                        self._drone._get_target_component(), 0,
                                        eb_mavutil.Enum.get_int_reference(
                                            "MAV_FRAME_LOCAL_NED"), COMMAND, 2,
                                        0, 0, 0, 0, 0,
                                        hold["current_pos"]["x"],
                                        hold["current_pos"]["y"],
                                        hold["current_pos"]["z"])

                    update_counter += 1

            hold["ended"] = True

            Logger.PrintLog(self.LOG_INFO,
                            "hold_local_position() - Handler has ended.")
Example #5
0
    def _thread_handler(cls) -> None:
        while cls._status:
            LOG_INFO = "gy_neo6mv2.py - _thread_handler()"

            try:
                gps_data = cls._serial.read_until(
                    GY_NEO6MV2.LINE_END).decode("ascii")
                data_split = gps_data.split(",")

                if "$GPGGA" != data_split[0]: continue

                cls._last_data = {
                    "timestamp": Time.get_current_timestamp("ms"),
                    "latitude": float(data_split[2]) / 100,  # Degree, north
                    "longitude": float(data_split[4]) / 100,  # Degree, east
                    "fix": int(data_split[6]),  # Fix quality
                    "sat_count":
                    int(data_split[7]),  # Number of satellites being tracked
                    "altitude":
                    float(data_split[9])  # Meters, above mean sea level
                }
            except ValueError:
                Logger.PrintLog(
                    LOG_INFO,
                    "Couldn't parse gps data. Perhaps module is still initializing?"
                )
            except Exception as ex:
                Logger.PrintException(LOG_INFO, ex)
                continue
Example #6
0
    def wait_mission_ack(self, timeout=5000):
        if timeout <= 0:
            raise ValueError("wait_mission_ack() - Timeout is <= 0.")

        start_timestamp = Time.get_current_timestamp("ms")

        while 1:
            if "MISSION_ACK" not in self._messages:
                if Time.get_current_timestamp("ms") - start_timestamp >= timeout:
                    raise TimeoutError("wait_mission_ack() timed out.")
                continue

            result = self._messages["MISSION_ACK"]["type"]

            del self._messages["MISSION_ACK"]
            return result
Example #7
0
        def _thread_handler(cls) -> None:
            while cls._status:
                try:
                    block_data = cls._read_block_data(0x00, 6)

                    if block_data[0] & 0x08:
                        pressure_raw = ((block_data[1] << 16
                                         | block_data[2] << 8
                                         | block_data[3]) >> 4) / 16.0

                        temp_raw = ((block_data[4] << 8
                                     | block_data[5]) >> 4) / 16.0

                        cls._last_data["temperature"] = temp_raw

                        if cls._next_measurement_type == 1:
                            cls._last_data["altitude"] = pressure_raw
                            cls._next_measurement_type = 2

                            cls._write_byte(MPL3115A2.REGISTER.CONTROL_1, 0x39)
                        elif cls._next_measurement_type == 2:
                            cls._last_data["pressure"] = pressure_raw * 16 / 4.0 / 1000.0
                            cls._next_measurement_type = 1

                            cls._write_byte(MPL3115A2.REGISTER.CONTROL_1, 0xB9)
                            sleep(1)

                        cls._last_data["timestamp"] = Time.get_current_timestamp("ms")
                except TypeError: pass

                sleep(1 / cls._data_rate)
Example #8
0
    def get_attitude(self):
        try:
            self._drone._attitude["last_update"] = Time.get_current_timestamp(
                "ms") - self._drone._attitude["timestamp"]
        except KeyError:
            pass

        return self._drone._attitude
Example #9
0
    def wait_cmd_ack(self, cmd, timeout=5000):
        if timeout <= 0:
            raise ValueError("wait_cmd_ack() - Timeout is <= 0.")

        start_timestamp = Time.get_current_timestamp("ms")

        while 1:
            if cmd not in self._messages["COMMAND_ACK"]:
                if Time.get_current_timestamp("ms") - start_timestamp >= timeout:
                    raise TimeoutError("wait_cmd_ack() for command {} has timed out."
                                       .format(str(cmd)))
                continue

            result = self._messages["COMMAND_ACK"][cmd]["result"]

            del self._messages["COMMAND_ACK"][cmd]
            return result
Example #10
0
    def get_last_data(self) -> Optional[dict]:
        if self._last_data == {}:
            return {
                "voltage": None,
                "timestamp": Time.get_current_timestamp("ms")
            }

        return self._last_data
Example #11
0
    def get_global_position(self):
        try:
            self._drone._global_position[
                "last_update"] = Time.get_current_timestamp(
                    "ms") - self._drone._global_position["timestamp"]
        except KeyError:
            pass

        return self._drone._global_position
Example #12
0
    def get_last_data(self) -> Optional[dict]:
        if self._last_data == {}:
            return {
                "timestamp"       : Time.get_current_timestamp("ms"),
                "distance"        : None,
                "signal_strength" : None,
                "mode"            : None
            }

        return self._last_data
Example #13
0
        def get_last_data(self) -> Optional[dict]:
            if self._last_data == {}:
                return {
                    "pressure"   : None,
                    "altitude"   : None,
                    "temperature": None,
                    "timestamp"  : Time.get_current_timestamp("ms")
                }

            return self._last_data
Example #14
0
        def until_value(func: Callable,
                        args: tuple,
                        timeout: int = 0,
                        repeat_delay_ms: Optional[int] = None,
                        ret_val: any = True) -> None:
            if  repeat_delay_ms is not None \
            and repeat_delay_ms <= 0:
                raise ValueError("delay <= 0")

            start_time = Time.get_current_timestamp("ms")

            while timeout == 0 \
            or    Time.get_current_timestamp("ms") - start_time < timeout:
                if func(*args) == ret_val: return

                if repeat_delay_ms is not None:
                    sleep(repeat_delay_ms / 1000)

            raise TimeoutError
Example #15
0
    def wait_msg(self, msg, timeout=5000):
        if timeout <= 0:
            raise ValueError("wait_msg() - Timeout is <= 0.")
        elif msg == "COMMAND_ACK":
            raise NotImplementedError

        start_timestamp = Time.get_current_timestamp("ms")

        while 1:
            if msg not in self._messages:
                if Time.get_current_timestamp("ms") - start_timestamp >= timeout:
                    raise TimeoutError("wait_msg() for message {} has timed out."
                                       .format(str(msg)))
                continue

            msg_data = self._messages[msg]

            del self._messages[msg]
            return msg_data
Example #16
0
 def get_last_data(self) -> Optional[dict]:
     if self._last_data == {}:
         return {
             "timestamp": Time.get_current_timestamp("ms"),
             "latitude": None,
             "longitude": None,
             "fix": None,
             "sat_count": None,
             "altitude": None
         }
     return self._last_data
Example #17
0
    def _thread_handler(cls) -> None:
        while cls._status:
            voltage = cls._adc.get_last_data()
            if voltage is None: continue

            voltage = voltage[cls._channel]["voltage"] / (10**3)  # V
            voltage = voltage * 5. / 1023.  # max471 conversion

            cls._last_data = {
                "voltage": voltage,
                "timestamp": Time.get_current_timestamp("ms")
            }
Example #18
0
    def _thread_handler(cls) -> None:
        while cls._status:
            for channel in cls._channels:
                if channel > 3: continue

                try:
                    mV = cls._adc.read_adc(channel, cls._gain)
                except OSError:
                    continue

                if "channel" not in cls._last_data:
                    cls._last_data["channel"] = {}

                cls._last_data[channel] = {
                    "voltage": mV,
                    "timestamp": Time.get_current_timestamp("ms")
                }

            sleep(1 / cls._data_rate)
Example #19
0
    def _thread_handler(cls) -> None:
        while cls._status:
            if cls._serial.in_waiting() >= 9:
                recv_data = cls._serial.read(9)

                if recv_data[:2] == b"\x59\x59":
                    distance        = (recv_data[3] << 8) | recv_data[2]
                    signal_strength = (recv_data[5] << 8) | recv_data[4]
                    mode            = recv_data[6]
                    checksum        = recv_data[8]

                    cls._last_data = {
                        "timestamp"       : Time.get_current_timestamp("ms"),
                        "distance"        : distance,
                        "signal_strength" : signal_strength,
                        "mode"            : mode
                    }

                cls._serial.flush_input_buffer()

        cls._serial.stop()
Example #20
0
        def __init__(self,
                     base_addr         : int = 0x68,
                     data_rate         : int = 10) -> None:
            self.LOG_INFO = "mpl3115a2.py"

            self._addr      = base_addr
            self._bus       = None
            self._sensor    = None
            self._data_rate = data_rate
            self._last_data = {
                "pressure"   : None,  # pascal (kPa)
                "altitude"   : None,  # meters
                "temperature": None,  # degree C,
                "timestamp"  : Time.get_current_timestamp("ms")
            }
            self._status    = 0
            self._next_measurement_type = 0 # 1- Altimeter, 2- Barometer

            try:
                self._bus = SMBus(1)
            except Exception as ex:
                Logger.PrintException(self.LOG_INFO + " - __init__()", ex)
                return
Example #21
0
    def get_last_data(self) -> Optional[dict]:
        if self._last_data == {}:
            return {
                "accel": {
                    "raw": {
                        "x": None,
                        "y": None,
                        "z": None
                    },
                    "scaled": {
                        "x": None,
                        "y": None,
                        "z": None
                    }
                },
                "gyro": {
                    "raw": {
                        "x": None,
                        "y": None,
                        "z": None
                    },
                    "scaled": {
                        "x": None,
                        "y": None,
                        "z": None
                    }
                },
                "mag": {
                    "x": None,
                    "y": None,
                    "z": None
                },
                "timestamp": Time.get_current_timestamp("ms")
            }

        return self._last_data
Example #22
0
    def get_battery(self):
        self._drone._battery["last_update"] = Time.get_current_timestamp(
            "ms") - self._drone._battery["timestamp"]

        return self._drone._battery
Example #23
0
 def get_throttle(self):
     return self._drone._throttle[0], Time.get_current_timestamp(
         "ms") - self._drone._throttle[1]
Example #24
0
    def get_last_heartbeat(self, ret_diff=False):
        if ret_diff:
            return Time.get_current_timestamp(
                "ms") - self._drone._last_heartbeat

        return self._drone._last_heartbeat
Example #25
0
 def get_boot_time(self):
     return self._drone._boot_time[0], Time.get_current_timestamp(
         "ms") - self._drone._boot_time[1]
Example #26
0
 def get_ground_speed(self):
     return self._drone._ground_speed[0], Time.get_current_timestamp(
         "ms") - self._drone._ground_speed[1]
Example #27
0
 def get_altitude(self):
     return self._drone._alt[0], Time.get_current_timestamp(
         "ms") - self._drone._alt[1]
Example #28
0
    def _thread_handler(cls):
        LOG_INFO = cls.LOG_INFO + " - _thread_handler()"

        Logger.PrintLog(LOG_INFO, "Mission tracker thread has started.")

        emergency_mode = "LAND"
        set_to_emergency_mode = False

        while cls.get_status() == 1:
            try:
                is_completed = False
                current_mission_index = cls.get_current_mission_index()
                current_mission = cls._mission_list[current_mission_index]
                current_flight_mode = cls._drone.telemetry().get_flight_mode()

                if current_flight_mode != "GUIDED" \
                and (current_flight_mode != "LAND" and current_mission.get("type") != Mission.Type.LAND):
                    Logger.PrintLog(
                        LOG_INFO,
                        "Flight mode has changed from GUIDED mode. Stopping. Current flight mode: {}."
                        .format(current_flight_mode))
                    break

                if current_mission.get("status") == 0:
                    global_pos = cls._drone.telemetry().get_global_position()

                    current_mission.set_storage_variable(
                        "rel_alt", global_pos["relative_alt"])
                    current_mission.set_storage_variable(
                        "start_ms", Time.get_current_timestamp("ms"))
                    current_mission.set_storage_variable("retries", 0)

                    res = cls._execute_mission(current_mission,
                                               cls._mission_retries,
                                               cls._mission_retry_timeout)

                    if not res:
                        Logger.PrintLog(
                            LOG_INFO,
                            "_execute_mission() has failed. Stopping. Current mission index: {}."
                            .format(str(current_mission_index)))

                        set_to_emergency_mode = True
                        break

                    Logger.PrintLog(
                        LOG_INFO,
                        "Tracking the mission at index {}, type {}.".format(
                            str(current_mission_index),
                            Mission.Type.get_str(current_mission.get("type"))))

                    cls._mission_list[current_mission_index].set("status", 1)
                # .\mission command execution

                if current_mission.get("type") == Mission.Type.TAKEOFF:
                    Logger.PrintLog(LOG_INFO,
                                    "Tracking TAKEOFF mission status.")

                    desired_alt = current_mission.get("alt")
                    threshold = 0.5

                    if cls._drone.control().is_reached_to_relative_alt(
                            desired_alt, threshold):
                        is_completed = True
                elif current_mission.get("type") == Mission.Type.LAND:
                    Logger.PrintLog(LOG_INFO, "Tracking LAND mission status.")
                    threshold = 0.5

                    if cls._drone.control().is_reached_to_relative_alt(
                            0, threshold):
                        is_completed = True
                elif current_mission.get("type") == Mission.Type.WAYPOINT:
                    Logger.PrintLog(LOG_INFO,
                                    "Tracking WAYPOINT mission status.")

                    dest_pos = {
                        "lat": current_mission.get("lat"),
                        "lon": current_mission.get("lon")
                    }

                    desired_alt = current_mission.get("alt")
                    threshold = 0.15

                    # /!\ Toggle below line if odd behaviours observed on drone.
                    cls._execute_mission(current_mission, cls._mission_retries,
                                         cls._mission_retry_timeout)

                    if cls._drone.control().is_reached_to_global_position(
                            dest_pos["lat"], dest_pos["lon"], desired_alt,
                            threshold, cls._wp_radius):
                        is_completed = True
                elif current_mission.get("type") == Mission.Type.HOME_POINT:
                    is_completed = True
                # .\mission controls

                if not is_completed:
                    # not used currently
                    current_mission.set_storage_variable(
                        "retries",
                        current_mission.get_storage_variable("retries") + 1)
                else:
                    Logger.PrintLog(
                        LOG_INFO,
                        "Mission at index {}, type {} has completed.".format(
                            str(cls.get_current_mission_index()),
                            Mission.Type.get_str(current_mission.get("type"))))

                    script = current_mission.get("script")

                    if script != "":
                        script = script + ".py"
                        script_path = cls._scripts_dir + script

                        if os.path.isfile(script_path):
                            with open(script_path, "r") as f:
                                Logger.PrintLog(
                                    LOG_INFO,
                                    "Executing script {}.".format(script))

                                cls._current_running_script = script
                                exec_return = {}

                                hold_pos = {"hold": True}
                                cls._drone.action().hold_global_position(
                                    hold_pos, cls._pos_update_interval)

                                try:
                                    exec(
                                        compile(f.read(), script, "exec"), {
                                            "drone": cls._drone,
                                            "control": {
                                                "position": {
                                                    "hold": hold_pos
                                                }
                                            }
                                        }, exec_return)
                                except Exception as ex:
                                    Logger.PrintLog(
                                        LOG_INFO,
                                        "An exception occured while running {}. Exception: {} - {}. Proceeding to next mission if there is."
                                        .format(script,
                                                type(ex).__name__, str(ex)))

                                Logger.PrintLog(
                                    LOG_INFO,
                                    "Script executed. Returned data: {}".
                                    format(
                                        str(exec_return.get("return_val",
                                                            {}))))

                                hold_pos["hold"] = False
                                Method.Dict.wait_until_value(
                                    hold_pos, "ended", True)
                        else:
                            Logger.PrintLog(
                                LOG_INFO,
                                "Cannot find script {} in scripts path ({}). Skipping."
                                .format(script, cls._scripts_dir))

                    cls._current_running_script = ""
                    cls._mission_list[current_mission_index].set("status", 2)

                    if current_mission_index != cls.get_mission_count() - 1:
                        cls._current_mission_index += 1
                    else:
                        Logger.PrintLog(
                            LOG_INFO,
                            "All missions are successfully finished.")

                        if cls._mission_list[-1].get(
                                "type") != Mission.Type.LAND:
                            set_to_emergency_mode = True

                        break

                    if current_mission.get("delay") != 0:
                        Logger.PrintLog(
                            LOG_INFO,
                            "Delaying the next mission for {} seconds.".format(
                                current_mission.get("delay")))

                        hold_pos = {"hold": True}
                        cls._drone.action().hold_global_position(
                            hold_pos, cls._pos_update_interval)

                        delay_timestamp = Time.get_current_timestamp("ms")

                        while 1:
                            time_diff = Time.get_current_timestamp(
                                "ms") - delay_timestamp

                            if time_diff >= current_mission.get(
                                    "delay") * 1000:
                                break

                        Logger.PrintLog(LOG_INFO, "Delay has ended.")

                        hold_pos["hold"] = False
                        Method.Dict.wait_until_value(hold_pos, "ended", True)

                    continue
                # .\is mission completed check

                sleep(1 / cls._control_rate)
            except Exception as ex:
                Logger.PrintException(LOG_INFO, ex)

                set_to_emergency_mode = True
                break
        # .\while

        if cls.get_status() == 2:
            Logger.PrintLog(
                LOG_INFO, "Mission at index {} is paused.".format(
                    str(cls.get_current_mission_index())))
            cls._mission_list[cls.get_current_mission_index()].set("status", 0)
            cls._mission_list[cls.get_current_mission_index()].set(
                "is_uploaded", 0)

        if cls.get_status() != 2:
            cls._status = 0

        if set_to_emergency_mode:
            Logger.PrintLog(
                LOG_INFO,
                "!!! EMERGENCY SITUATION DETECTED !!! Setting flight mode to {}."
                .format(emergency_mode))
            cls._drone.action().set_flight_mode(emergency_mode)

        Logger.PrintLog(LOG_INFO, "Mission tracker thread has ended.")
Example #29
0
 def get_climb_rate(self):
     return self._drone._climb_rate[0], Time.get_current_timestamp(
         "ms") - self._drone._climb_rate[1]
Example #30
0
 def get_heading(self):
     return self._drone._heading[0], Time.get_current_timestamp(
         "ms") - self._drone._heading[1]