class BalloonStrategy(object):
    def __init__(self):

        # connect to vehicle with dronekit
        self.vehicle = self.get_vehicle_with_dronekit()

        # initialised flag
        self.home_initialised = False
        # timer to intermittently check for home position
        self.last_home_check = time.time()

        # historical attitude
        self.att_hist = AttitudeHistory(self.vehicle, 2.0)
        self.attitude_delay = 0.0  # expected delay between image and attitude

        # search variables
        self.search_state = 0  # 0 = not search, 1 = spinning and looking, 2 = rotating to best balloon and double checking, 3 = finalise yaw
        self.search_start_heading = None  # initial heading of vehicle when search began
        self.search_target_heading = None  # the vehicle's current target heading (updated as vehicle spins during search)
        self.search_heading_change = None  # heading change (in radians) performed so far during search
        self.search_balloon_pos = None  # position (as an offset from home) of closest balloon (so far) during search
        self.search_balloon_heading = None  # earth-frame heading (in radians) from vehicle to closest balloon
        self.search_balloon_pitch_top = None  # earth-frame pitch (in radians) from vehicle to closest balloon
        self.search_balloon_distance = None  # distance (in meters) from vehicle to closest balloon
        self.targeting_start_time = 0  # time vehicle first pointed towards final target (used with delay_time below)
        self.targeting_delay_time = balloon_config.config.get_float(
            'general', 'SEARCH_TARGET_DELAY', 2.0
        )  # time vehicle waits after pointing towards ballon and before heading towards it
        self.search_yaw_speed = balloon_config.config.get_float(
            'general', 'SEARCH_YAW_SPEED', 5.0)

        # vehicle mission
        self.mission_cmds = None
        self.mission_alt_min = 0  # min altitude from NAV_GUIDED mission command (we ignore balloons below this altitude).  "0" means no limit
        self.mission_alt_max = 0  # max altitude from NAV_GUIDED mission command (we ignore balloons above this altitude).  "0" means no limit
        self.mission_distance_max = 0  # max distance from NAV_GUIDED mission command (we ignore balloons further than this distance).  "0" means no limit

        # we are not in control of vehicle
        self.controlling_vehicle = False

        # vehicle position captured at time camera image was captured
        self.vehicle_pos = None

        # balloon direction and position estimate from latest call to analyse_image
        self.balloon_found = False
        self.balloon_pitch = None
        self.balloon_pitch_top = None  # earth-frame pitch (in radians) from vehicle to top of closest balloon
        self.balloon_heading = None
        self.balloon_distance = None
        self.balloon_pos = None  # last estimated position as an offset from home

        # time of the last target update sent to the flight controller
        self.guided_last_update = time.time()

        # latest velocity target sent to flight controller
        self.guided_target_vel = None

        # time the target balloon was last spotted
        self.last_spotted_time = 0

        # if we lose sight of a balloon for this many seconds we will consider it lost and give up on the search
        self.lost_sight_timeout = 3

        # The module only prints log messages unless the vehicle is in GUIDED mode (for testing).
        # Once this seems to work reasonablly well change self.debug to False and then it will
        # actually _enter_ guided mode when it thinks it sees a balloon
        self.debug = balloon_config.config.get_boolean('general', 'debug',
                                                       True)

        # use the simulator to generate fake balloon images
        self.use_simulator = balloon_config.config.get_boolean(
            'general', 'simulate', False)

        # start background image grabber
        if not self.use_simulator:
            balloon_video.start_background_capture()

        # initialise video writer
        self.writer = None

        # horizontal velocity pid controller.  maximum effect is 10 degree lean
        xy_p = balloon_config.config.get_float('general', 'VEL_XY_P', 1.0)
        xy_i = balloon_config.config.get_float('general', 'VEL_XY_I', 0.0)
        xy_d = balloon_config.config.get_float('general', 'VEL_XY_D', 0.0)
        xy_imax = balloon_config.config.get_float('general', 'VEL_XY_IMAX',
                                                  10.0)
        self.vel_xy_pid = pid.pid(xy_p, xy_i, xy_d, math.radians(xy_imax))

        # vertical velocity pid controller.  maximum effect is 10 degree lean
        z_p = balloon_config.config.get_float('general', 'VEL_Z_P', 2.5)
        z_i = balloon_config.config.get_float('general', 'VEL_Z_I', 0.0)
        z_d = balloon_config.config.get_float('general', 'VEL_Z_D', 0.0)
        z_imax = balloon_config.config.get_float('general', 'VEL_IMAX', 10.0)
        self.vel_z_pid = pid.pid(z_p, z_i, z_d, math.radians(z_imax))

        # velocity controller min and max speed
        self.vel_speed_min = balloon_config.config.get_float(
            'general', 'VEL_SPEED_MIN', 1.0)
        self.vel_speed_max = balloon_config.config.get_float(
            'general', 'VEL_SPEED_MAX', 4.0)
        self.vel_speed_last = 0.0  # last recorded speed
        self.vel_accel = balloon_config.config.get_float(
            'general', 'VEL_ACCEL', 0.5)  # maximum acceleration in m/s/s
        self.vel_dist_ratio = balloon_config.config.get_float(
            'general', 'VEL_DIST_RATIO', 0.5)

        # pitch angle to hit balloon at.  negative means come down from above
        self.vel_pitch_target = math.radians(
            balloon_config.config.get_float('general', 'VEL_PITCH_TARGET',
                                            -5.0))

        # velocity controller update rate
        self.vel_update_rate = balloon_config.config.get_float(
            'general', 'VEL_UPDATE_RATE_SEC', 0.2)

        # stats
        self.num_frames_analysed = 0
        self.stats_start_time = 0

    # connect to vehicle with dronekit
    def get_vehicle_with_dronekit(self):
        #connection_str = balloon_config.config.get_string('dronekit','connection_string','0.0.0.0:14550')
        #connection_baud = balloon_config.config.get_integer('dronekit','baud', 921600)
        print "connecting to vehicle"  # on %s, baud=%d" % (connection_str, connection_baud)
        #return connect(connection_str, baud=connection_baud)
        return connect('0.0.0.0:14550', wait_ready=True)
        print "connected to vehicle"

    # fetch_mission - fetch mission from flight controller
    def fetch_mission(self):
        # download the vehicle waypoints
        print "fetching mission.."
        self.mission_cmds = self.vehicle.commands
        self.mission_cmds.download()
        self.mission_cmds.wait_ready()
        if not self.mission_cmds is None:
            print "retrieved mission with %d commands" % self.mission_cmds.count
        else:
            print "failed to retrieve mission"

    # check home - intermittently checks for changes to the home location
    def check_home(self):

        # return immediately if home has already been initialised
        if self.home_initialised:
            return True

        # check for home no more than once every two seconds
        if (time.time() - self.last_home_check > 2):

            # update that we have performed a status check
            self.last_home_check = time.time()

            # check if we have a vehicle
            if self.vehicle is None:
                self.vehicle = self.get_vehicle_with_dronekit()
                return

            # download the vehicle waypoints (and home) if we don't have them already
            if self.mission_cmds is None:
                self.fetch_mission()
                return False

            # ensure the vehicle's position is known
            if self.vehicle.location.global_relative_frame is None:
                print "waiting for vehicle position.."
                return False
            if self.vehicle.location.global_relative_frame.lat is None or self.vehicle.location.global_relative_frame.lon is None or self.vehicle.location.global_relative_frame.alt is None:
                print "waiting for vehicle position.."
                return False

            # get home location from mission command list
            if self.vehicle.home_location is None:
                print "waiting for home location.."
                self.fetch_mission()
                return False

            # sanity check home position
            if self.vehicle.home_location.lat == 0 and self.vehicle.home_location.lon == 0:
                print "home position all zero, re-fetching.."
                self.fetch_mission()
                return False

            # set home position
            PositionVector.set_home_location(
                LocationGlobal(self.vehicle.home_location.lat,
                               self.vehicle.home_location.lon, 0))
            self.home_initialised = True
            print "got home location"

            # To-Do: if we wish to have the same home position as the flight controller
            # we must download the home waypoint again whenever the vehicle is armed

        # return whether home has been initialised or not
        return self.home_initialised

    # checks if video output should be started
    def check_video_out(self):

        # return immediately if video has already been started
        if not self.writer is None:
            return

        # start video once vehicle is armed
        if self.vehicle.armed:
            self.writer = balloon_video.open_video_writer()

    # check_status - poles vehicle' status to determine if we are in control of vehicle or not
    def check_status(self):

        # we are active in guided mode
        if self.vehicle.mode.name == "GUIDED":
            if not self.controlling_vehicle:
                self.controlling_vehicle = True
                print "taking control of vehicle in GUIDED"
                # clear out any limits on balloon position
                self.mission_alt_min = 1
                self.mission_alt_max = 0
                self.mission_distance_max = 50
                # start search for balloon
                print "starting search alt_min:%f alt_max:%f dist_max:%f" % (
                    self.mission_alt_min, self.mission_alt_max,
                    self.mission_distance_max)
                self.start_search()
            return

        # download the vehicle waypoints if we don't have them already
        # To-Do: do not load waypoints if vehicle is armed
        if self.mission_cmds is None:
            self.fetch_mission()
            return

        # Check for Auto mode and executing Nav-Guided command
        if self.vehicle.mode.name == "AUTO":

            # get active command number and mavlink id
            active_command = self.vehicle.commands.next
            active_command_id = self.vehicle.commands[active_command].command

            # ninety two is the MAVLink id for Nav-Guided-Enable commands
            if active_command_id == 92:
                if not self.controlling_vehicle:
                    self.controlling_vehicle = True
                    print "taking control of vehicle in AUTO"
                    self.mission_alt_min = self.vehicle.commands[
                        active_command].param2
                    self.mission_alt_max = self.vehicle.commands[
                        active_command].param3
                    self.mission_distance_max = self.vehicle.commands[
                        active_command].param4
                    print "starting search alt_min:%f alt_max:%f dist_max:%f" % (
                        self.mission_alt_min, self.mission_alt_max,
                        self.mission_distance_max)
                    self.start_search()
                return

        # if we got here then we are not in control
        if self.controlling_vehicle:
            self.controlling_vehicle = False
            print "giving up control of vehicle in %s" % self.vehicle.mode.name

    # condition_yaw - send condition_yaw mavlink command to vehicle so it points at specified heading (in degrees)
    def condition_yaw(self, heading):
        # create the CONDITION_YAW command
        msg = self.vehicle.message_factory.mission_item_encode(
            0,
            0,  # target system, target component
            0,  # sequence
            mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,  # frame
            mavutil.mavlink.MAV_CMD_CONDITION_YAW,  # command
            2,  # current - set to 2 to make it a guided command
            0,  # auto continue
            heading,
            0,
            0,
            0,
            0,
            0,
            0)  # param 1 ~ 7
        # send command to vehicle
        self.vehicle.send_mavlink(msg)
        self.vehicle.flush()

    # send_nav_velocity - send nav_velocity command to vehicle to request it fly in specified direction
    def send_nav_velocity(self, velocity_x, velocity_y, velocity_z):
        # create the SET_POSITION_TARGET_LOCAL_NED command
        msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
            0,  # time_boot_ms (not used)
            0,
            0,  # target system, target component
            mavutil.mavlink.MAV_FRAME_LOCAL_NED,  # frame
            0b0000111111000111,  # type_mask (only speeds enabled)
            0,
            0,
            0,  # x, y, z positions (not used)
            velocity_x,
            velocity_y,
            velocity_z,  # x, y, z velocity in m/s
            0,
            0,
            0,  # x, y, z acceleration (not used)
            0,
            0)  # yaw, yaw_rate (not used)
        # send command to vehicle
        self.vehicle.send_mavlink(msg)
        self.vehicle.flush()

    # advance_current_cmd - ask vehicle to advance to next command (i.e. abort current command)
    def advance_current_cmd(self):

        # exit immediately if we are not in AUTO mode or not controlling the vehicle
        if not self.vehicle.mode.name == "AUTO" or not self.controlling_vehicle:
            return

        # download the vehicle waypoints if we don't have them already
        if self.mission_cmds is None:
            self.fetch_mission()

        # get active command
        active_command = self.vehicle.commands.next

        # ensure there is one more command at least
        if (self.vehicle.commands.count > active_command):
            # create the MISSION_SET_CURRENT command
            msg = self.vehicle.message_factory.mission_set_current_encode(
                0, 0, active_command +
                1)  # target system, target component, sequence
            # send command to vehicle
            self.vehicle.send_mavlink(msg)
            self.vehicle.flush()
        else:
            print "Failed to advance command"

    # get_frame - get a single frame from the camera or simulator
    def get_frame(self):
        if self.use_simulator:
            veh_pos = PositionVector.get_from_location(
                self.vehicle.location.global_relative_frame)
            frame = balloon_sim.get_simulated_frame(
                veh_pos, self.vehicle.attitude.roll,
                self.vehicle.attitude.pitch, self.vehicle.attitude.yaw)
        else:
            frame = balloon_video.get_image()
        return frame

    # get image from balloon_video class and look for balloon, results are held in the following variables:
    #    self.balloon_found : set to True if balloon is found, False otherwise
    #    self.balloon_pitch : earth frame pitch (in radians) from vehicle to balloon (i.e. takes account of vehicle attitude)
    #    self.balloon_heading : earth frame heading (in radians) from vehicle to balloon
    #    self.balloon_distance : distance (in meters) from vehicle to balloon
    #    self.balloon_pos : position vector of balloon's position as an offset from home in meters
    def analyze_image(self):

        # record time
        now = time.time()

        # capture vehicle position
        self.vehicle_pos = PositionVector.get_from_location(
            self.vehicle.location.global_relative_frame)

        # capture vehicle attitude in buffer
        self.att_hist.update()

        # get delayed attitude from buffer
        veh_att_delayed = self.att_hist.get_attitude(now - self.attitude_delay)

        # get new image from camera
        f = self.get_frame()

        # look for balloon in image using blob detector
        self.balloon_found, xpos, ypos, size = balloon_finder.analyse_frame(f)

        # add artificial horizon
        balloon_finder.add_artificial_horizon(f, veh_att_delayed.roll,
                                              veh_att_delayed.pitch)

        if self.balloon_found:
            # record time balloon was found
            self.last_spotted_time = now

            # convert x, y position to pitch and yaw direction (in radians)
            self.balloon_pitch, self.balloon_heading = balloon_finder.pixels_to_direction(
                xpos, ypos, veh_att_delayed.roll, veh_att_delayed.pitch,
                veh_att_delayed.yaw)
            self.balloon_pitch = math.radians(self.balloon_pitch)
            self.balloon_pitch_top = self.balloon_pitch  # + balloon_video.pixels_to_angle_y(size)  # add balloon radius so we aim for top of balloon
            self.balloon_heading = math.radians(self.balloon_heading)

            # get distance
            self.balloon_distance = get_distance_from_pixels(
                size, balloon_finder.balloon_radius_expected)

            # updated estimated balloon position
            self.balloon_pos = balloon_finder.project_position(
                self.vehicle_pos, self.balloon_pitch, self.balloon_heading,
                self.balloon_distance)

        # save image for debugging later
        if not self.writer is None:
            self.writer.write(f)

        # increment frames analysed for stats
        self.num_frames_analysed += 1
        if self.stats_start_time == 0:
            self.stats_start_time = time.time()

    # start_search - start search for balloon
    def start_search(self):
        # exit immediately if we are not controlling the vehicle
        if not self.controlling_vehicle:
            return
        # initialise search variables
        self.search_state = 1  # 0 = not search, 1 = spinning and looking, 2 = rotating to best balloon and double checking, 3 = finalise yaw
        self.search_balloon_pos = None
        self.search_start_heading = self.vehicle.attitude.yaw
        self.search_target_heading = self.search_start_heading
        self.search_total_angle = 0
        self.targeting_start_time = 0
        # reset vehicle speed
        self.vel_speed_last = 0

    # search - spin vehicle looking for balloon
    # analyze_image should have been called just before and should have filled in self.balloon_found, self.balloon_pos, balloon_distance, balloon_pitch and balloon_yaw
    def search_for_balloon(self):

        # exit immediately if we are not controlling the vehicle or do not know our position
        if not self.controlling_vehicle or self.vehicle_pos is None:
            return

        # exit immediately if we are not searching
        if self.search_state <= 0:
            return

        # search states: 0 = not searching, 1 = spinning and looking, 2 = rotating to best balloon and double checking, 3 = finalise yaw

        # search_state = 1: spinning and looking
        if (self.search_state == 1):

            # check if we have seen a balloon
            if self.balloon_found:
                # if this is the first balloon we've found or the closest, store it's position as the closest
                if (self.search_balloon_pos is None) or (
                        self.balloon_distance < self.search_balloon_distance):
                    # check distance is within acceptable limits
                    if (self.mission_alt_min == 0
                            or self.balloon_pos.z >= self.mission_alt_min
                        ) and (self.mission_alt_max == 0
                               or self.balloon_pos.z <= self.mission_alt_max
                               ) and (self.mission_distance_max == 0
                                      or self.balloon_distance <=
                                      self.mission_distance_max):
                        # record this balloon as the closest
                        self.search_balloon_pos = self.balloon_pos
                        self.search_balloon_heading = self.balloon_heading
                        self.search_balloon_pitch_top = self.balloon_pitch_top  # we target top of balloon
                        self.search_balloon_distance = self.balloon_distance
                        print "Found Balloon at heading:%f Alt:%f Dist:%f" % (
                            math.degrees(self.balloon_heading),
                            self.balloon_pos.z, self.balloon_distance)
                    else:
                        print "Balloon Ignored at heading:%f Alt:%f Dist:%f" % (
                            math.degrees(self.balloon_heading),
                            self.balloon_pos.z, self.balloon_distance)

            # update yaw so we keep spinning
            if math.fabs(
                    wrap_PI(self.vehicle.attitude.yaw -
                            self.search_target_heading)) < math.radians(
                                self.search_yaw_speed * 2.0):
                # increase yaw target
                self.search_target_heading = self.search_target_heading - math.radians(
                    self.search_yaw_speed)
                self.search_total_angle = self.search_total_angle + math.radians(
                    self.search_yaw_speed)
                # send yaw heading
                self.condition_yaw(math.degrees(self.search_target_heading))

                # end search if we've gone all the way around
                if self.search_total_angle >= math.radians(360):
                    # if we never saw a balloon then just complete (return control to user or mission)
                    if self.search_balloon_pos is None:
                        print "Search did not find balloon, giving up"
                        self.search_state = 0
                        self.complete()
                    else:
                        # update target heading towards closest balloon and send to flight controller
                        self.search_target_heading = self.search_balloon_heading
                        self.condition_yaw(
                            math.degrees(self.search_target_heading))
                        self.search_state = 2  # advance towards rotating to best balloon stage
                        print "best balloon at %f" % math.degrees(
                            self.search_balloon_heading)

        # search_state = 2: rotating to best balloon and double checking
        elif (self.search_state == 2):
            # check yaw is close to target
            if math.fabs(
                    wrap_PI(self.vehicle.attitude.yaw -
                            self.search_target_heading)) < math.radians(5):
                # reset targeting start time
                if self.targeting_start_time == 0:
                    self.targeting_start_time = time.time()

                # if targeting time has elapsed, double check the visible balloon is within acceptable limits
                if (time.time() - self.targeting_start_time >
                        self.targeting_delay_time):
                    if self.balloon_found and (
                            not self.balloon_pos is None
                    ) and (self.mission_alt_min == 0
                           or self.balloon_pos.z >= self.mission_alt_min) and (
                               self.mission_alt_max == 0
                               or self.balloon_pos.z <= self.mission_alt_max
                           ) and (self.mission_distance_max == 0
                                  or self.balloon_distance <=
                                  self.mission_distance_max):
                        # balloon is within limits so reset balloon target
                        self.search_balloon_pos = self.balloon_pos
                        self.search_balloon_heading = self.balloon_heading
                        self.search_balloon_pitch_top = self.balloon_pitch_top
                        self.search_balloon_distance = self.balloon_distance
                        # move to final heading
                        self.search_target_heading = self.search_balloon_heading
                        self.condition_yaw(
                            math.degrees(self.search_target_heading))
                        # move to finalise yaw state
                        self.search_state = 3
                        print "Balloon was near expected heading, finalising yaw to %f" % (
                            math.degrees(self.search_balloon_heading))

                    # we somehow haven't found the balloon when we've turned back to find it so giveup
                    elif (time.time() - self.targeting_start_time) > (
                            self.targeting_delay_time + 1.0):
                        print "Balloon was not at expected heading: %f, giving up" % (
                            math.degrees(self.search_target_heading))
                        self.search_state = 0
                        self.complete()

        # search_state = 3: finalising yaw
        elif (self.search_state == 3):
            # check yaw is close to target
            if math.fabs(
                    wrap_PI(self.vehicle.attitude.yaw -
                            self.search_target_heading)) < math.radians(5):
                # complete search, move should take-over
                # Note: we don't check again for the balloon, but surely it's still there
                self.search_state = 0
                print "Finalised yaw to %f, beginning run" % (math.degrees(
                    self.search_balloon_heading))

    # move_to_balloon - velocity controller to drive vehicle to balloon
    #    analyze_image should have been called prior to this and filled in self.balloon_found, balloon_pitch, balloon_heading, balloon_distance
    def move_to_balloon(self):

        # exit immediately if we are not controlling the vehicle
        if not self.controlling_vehicle:
            return

        # get current time
        now = time.time()

        # exit immediately if it's been too soon since the last update
        if (now - self.guided_last_update) < self.vel_update_rate:
            return

        # if we have a new balloon position recalculate velocity vector
        if (self.balloon_found):

            # calculate change in yaw since we began the search
            yaw_error = wrap_PI(self.balloon_heading -
                                self.search_balloon_heading)

            # calculate pitch vs ideal pitch angles.  This will cause to attempt to get to 5deg above balloon
            pitch_error = wrap_PI(self.balloon_pitch_top -
                                  self.vel_pitch_target)

            # get time since last time velocity pid controller was run
            dt = self.vel_xy_pid.get_dt(2.0)

            # get speed towards balloon based on balloon distance
            speed = self.balloon_distance * self.vel_dist_ratio

            # apply min and max speed limit
            speed = min(speed, self.vel_speed_max)
            speed = max(speed, self.vel_speed_min)

            # apply acceleration limit
            speed_chg_max = self.vel_accel * dt
            speed = min(speed, self.vel_speed_last + speed_chg_max)
            speed = max(speed, self.vel_speed_last - speed_chg_max)

            # record speed for next iteration
            self.vel_speed_last = speed

            # calculate yaw correction and final yaw movement
            yaw_correction = self.vel_xy_pid.get_pid(yaw_error, dt)
            yaw_final = wrap_PI(self.search_balloon_heading + yaw_correction)

            # calculate pitch correction and final pitch movement
            pitch_correction = self.vel_z_pid.get_pid(pitch_error, dt)
            pitch_final = wrap_PI(self.search_balloon_pitch_top -
                                  pitch_correction)

            # calculate velocity vector we wish to move in
            self.guided_target_vel = balloon_finder.get_ef_velocity_vector(
                pitch_final, yaw_final, speed)

            # send velocity vector to flight controller
            self.send_nav_velocity(self.guided_target_vel[0],
                                   self.guided_target_vel[1],
                                   self.guided_target_vel[2])
            self.guided_last_update = now

        # if have not seen the balloon
        else:
            # if more than a few seconds has passed without seeing the balloon give up
            if now - self.last_spotted_time > self.lost_sight_timeout:
                if self.debug:
                    print "Lost Balloon, Giving up"
                # To-Do: start searching again or maybe slowdown?
                self.complete()

        # FIXME - check if vehicle altitude is too low
        # FIXME - check if we are too far from the desired flightplan

    def output_frame_rate_stats(self):
        # get current time
        now = time.time()

        # output stats each 10seconds
        time_diff = now - self.stats_start_time
        if time_diff < 10 or time_diff <= 0:
            return

        # output frame rate
        frame_rate = self.num_frames_analysed / time_diff
        print "FrameRate: %f (%d frames in %f seconds)" % (
            frame_rate, self.num_frames_analysed, time_diff)

        # reset stats
        self.num_frames_analysed = 0
        self.stats_start_time = now

    def run(self):
        while True:

            # only process images once home has been initialised
            if self.check_home():

                # start video if required
                self.check_video_out()

                # check if we are controlling the vehicle
                self.check_status()

                # look for balloon in image
                self.analyze_image()

                # search or move towards balloon
                if self.search_state > 0:
                    # search for balloon
                    self.search_for_balloon()
                else:
                    # move towards balloon
                    self.move_to_balloon()

                # output stats
                self.output_frame_rate_stats()

            # Don't suck up too much CPU, only process a new image occasionally
            time.sleep(0.05)

        if not self.use_simulator:
            balloon_video.stop_background_capture()

    # complete - balloon strategy has somehow completed so return control to the autopilot
    def complete(self):
        # debug
        if self.debug:
            print "Complete!"

        # stop the vehicle and give up control
        if self.controlling_vehicle:
            self.guided_target_vel = (0, 0, 0)
            self.send_nav_velocity(self.guided_target_vel[0],
                                   self.guided_target_vel[1],
                                   self.guided_target_vel[2])
            self.guided_last_update = time.time()

        # if in GUIDED mode switch back to LOITER
        if self.vehicle.mode.name == "GUIDED":
            self.vehicle.mode = VehicleMode("LOITER")
            self.vehicle.flush()

        # if in AUTO move to next command
        if self.vehicle.mode.name == "AUTO":
            self.advance_current_cmd()

        # flag we are not in control of the vehicle
        self.controlling_vehicle = False

        return
Beispiel #2
0
class BalloonStrategy(object):
    def __init__(self):

        # connect to vehicle with dronekit
        self.vehicle = self.get_vehicle_with_dronekit()
        # mode 0 = balloon finder ; mode 1 = Precision Land & Landing on moving platform ; mode 2 = detection & following mode
        # mode 3 = Precision Land without timout ; mode 4 = Fire Detetction
        self.mission_on_guide_mode = balloon_config.config.get_integer('general','mode_on_guide',4)

        self.camera_width = balloon_config.config.get_integer('camera','width',640)
        self.camera_height = balloon_config.config.get_integer('camera','height',480)
        self.camera_hfov = balloon_config.config.get_float('camera', 'horizontal-fov', 72.42)
        self.camera_vfov = balloon_config.config.get_float('camera', 'vertical-fov', 43.3)

        # initialised flag
        self.home_initialised = False
        # timer to intermittently check for home position
        self.last_home_check = time.time()

        # historical attitude
        self.att_hist = AttitudeHistory(self.vehicle, 2.0)
        self.attitude_delay = 0.0               # expected delay between image and attitude

        # search variables
        self.search_state = 0                   # 0 = not search, 1 = spinning and looking, 2 = rotating to best balloon and double checking, 3 = finalise yaw 
        self.search_start_heading = None        # initial heading of vehicle when search began
        self.search_target_heading = None       # the vehicle's current target heading (updated as vehicle spins during search)
        self.search_heading_change = None       # heading change (in radians) performed so far during search
        self.search_balloon_pos = None          # position (as an offset from home) of closest balloon (so far) during search
        self.search_balloon_heading = None      # earth-frame heading (in radians) from vehicle to closest balloon
        self.search_balloon_pitch_top = None        # earth-frame pitch (in radians) from vehicle to closest balloon
        self.search_balloon_distance = None     # distance (in meters) from vehicle to closest balloon
        self.targeting_start_time = 0           # time vehicle first pointed towards final target (used with delay_time below)
        self.targeting_delay_time = balloon_config.config.get_float('general','SEARCH_TARGET_DELAY',2.0)    # time vehicle waits after pointing towards ballon and before heading towards it
        self.search_yaw_speed = balloon_config.config.get_float('general','SEARCH_YAW_SPEED',5.0) 

        # vehicle mission
        self.mission_cmds = None
        self.mission_alt_min = 0                # min altitude from NAV_GUIDED mission command (we ignore balloons below this altitude).  "0" means no limit
        self.mission_alt_max = 0                # max altitude from NAV_GUIDED mission command (we ignore balloons above this altitude).  "0" means no limit
        self.mission_distance_max = 0           # max distance from NAV_GUIDED mission command (we ignore balloons further than this distance).  "0" means no limit

        # we are not in control of vehicle
        self.controlling_vehicle = False

        # vehicle position captured at time camera image was captured
        self.vehicle_pos = None

        # balloon direction and position estimate from latest call to analyse_image
        self.balloon_found = False
        self.fire_found = False
        self.balloon_pitch = None
        self.balloon_pitch_top = None           # earth-frame pitch (in radians) from vehicle to top of closest balloon
        self.balloon_heading = None
        self.balloon_distance = None
        self.balloon_pos = None             # last estimated position as an offset from home

        # time of the last target update sent to the flight controller
        self.guided_last_update = time.time()

        # latest velocity target sent to flight controller
        self.guided_target_vel = None

        # time the target balloon was last spotted
        self.last_spotted_time = 0

        # if we lose sight of a balloon for this many seconds we will consider it lost and give up on the search
        self.lost_sight_timeout = 15

        # The module only prints log messages unless the vehicle is in GUIDED mode (for testing).
        # Once this seems to work reasonablly well change self.debug to False and then it will
        # actually _enter_ guided mode when it thinks it sees a balloon
        self.debug = balloon_config.config.get_boolean('general','debug',True)

        # use the simulator to generate fake balloon images
        self.use_simulator = balloon_config.config.get_boolean('general','simulate',False)

        # start background image grabber
        if not self.use_simulator:
            balloon_video.start_background_capture()

        # initialise video writer
        self.writer = None

        # horizontal velocity pid controller.  maximum effect is 10 degree lean
        xy_p = balloon_config.config.get_float('general','VEL_XY_P',1.0)
        xy_i = balloon_config.config.get_float('general','VEL_XY_I',0.0)
        xy_d = balloon_config.config.get_float('general','VEL_XY_D',0.0)
        xy_imax = balloon_config.config.get_float('general','VEL_XY_IMAX',10.0)
        self.vel_xy_pid = pid.pid(xy_p, xy_i, xy_d, math.radians(xy_imax))

        # vertical velocity pid controller.  maximum effect is 10 degree lean
        z_p = balloon_config.config.get_float('general','VEL_Z_P',2.5)
        z_i = balloon_config.config.get_float('general','VEL_Z_I',0.0)
        z_d = balloon_config.config.get_float('general','VEL_Z_D',0.0)
        z_imax = balloon_config.config.get_float('general','VEL_IMAX',10.0)
        self.vel_z_pid = pid.pid(z_p, z_i, z_d, math.radians(z_imax))

        # velocity controller min and max speed
        self.vel_speed_min = balloon_config.config.get_float('general','VEL_SPEED_MIN',1.0)
        self.vel_speed_max = balloon_config.config.get_float('general','VEL_SPEED_MAX',4.0)
        self.vel_speed_last = 0.0   # last recorded speed
        self.vel_accel = balloon_config.config.get_float('general','VEL_ACCEL', 0.5)    # maximum acceleration in m/s/s
        self.vel_dist_ratio = balloon_config.config.get_float('general','VEL_DIST_RATIO', 0.5) 

        # pitch angle to hit balloon at.  negative means come down from above
        self.vel_pitch_target = math.radians(balloon_config.config.get_float('general','VEL_PITCH_TARGET',-5.0))

        # velocity controller update rate
        self.vel_update_rate = balloon_config.config.get_float('general','VEL_UPDATE_RATE_SEC',0.2)

        # stats
        self.num_frames_analysed = 0
        self.stats_start_time = 0
        #self.attitude = (0,0,0)


        #FROM PRECISION LAND!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        #how many times to attempt a land before giving up
        self.search_attempts = balloon_config.config.get_integer('general','search_attempts', 5)

        #The timeout between losing the target and starting a climb/scan
        self.settle_time = balloon_config.config.get_integer('general','settle_time', 1.5)

        #how high to climb in meters to complete a scan routine
        self.climb_altitude = balloon_config.config.get_integer('general','climb_altitude', 20)

        #the max horizontal speed sent to autopilot
        #self.vel_speed_max = balloon_config.config.get_float('general', 'vel_speed_max', 5)

        #P term of the horizontal distance to velocity controller
        self.dist_to_vel = balloon_config.config.get_float('general', 'dist_to_vel', 0.15)

        #Descent velocity
        self.descent_rate = balloon_config.config.get_float('general','descent_rate', 0.5)

        #roll/pitch valupixee that is considered stable
        self.stable_attitude = balloon_config.config.get_float('general', 'stable_attitude', 0.18)

        #Climb rate when executing a search
        self.climb_rate = balloon_config.config.get_float('general','climb_rate', -2.0)

        #The height at a climb is started if no target is detected
        self.abort_height = balloon_config.config.get_integer('general', 'abort_height', 10)

        #when we have lock on target, only descend if within this radius
        self.descent_radius = balloon_config.config.get_float('general', 'descent_radius', 1.0)

        #The height at which we lock the position on xy axis, default = 1
        self.landing_area_min_alt = balloon_config.config.get_integer('general', 'landing_area_min_alt', 0)

        #The radius of the cylinder surrounding the landing pad
        self.landing_area_radius = balloon_config.config.get_integer('general', 'landing_area_radius', 20)

        #Whether the landing program can be reset after it is disabled
        self.allow_reset = balloon_config.config.get_boolean('general', 'allow_reset', True)

        #Run the program no matter what mode or location; Useful for debug purposes
        self.always_run = balloon_config.config.get_boolean('general', 'always_run', True)

        #whether the companion computer has control of the autopilot or not
        self.in_control = False

        #The timeout between losing the target and starting a climb/scan
        self.settle_time = balloon_config.config.get_integer('general','settle_time', 1.5)
        
        #how many frames have been captured
        self.frame_count = 0
        
        #Reset state machine
        self.initialize_landing()

        # Variable sendiri
        self.lanjut_cmd = 1
        self.arm = True
        self.a = False
        self.b = 0
        self.c = False
        self.relay(0)  #servo
        self.relay1(0) #indikator
        self.relay2(0) #magnet
        self.servo(0)
        self.waktu = 0
        self.waktu1 = 0
        self.drop = 0
        #GPIO.output(16,GPIO.HIGH)
        #GPIO.output(20,GPIO.LOW)
    
    # connect to vehicle with dronekit
    def get_vehicle_with_dronekit(self):
        connection_str = balloon_config.config.get_string('dronekit','connection_string','/dev/ttyUSB0') 
        connection_baud = balloon_config.config.get_integer('dronekit','baud',921600)
        #connection_baud = balloon_config.config.get_integer('dronekit','baud',1500000)
        print "connecting to vehicle on %s, baud=%d" % (connection_str, connection_baud)
        return connect(connection_str, baud=connection_baud)
        print "connected to vehicle"

    # fetch_mission - fetch mission from flight controller
    def fetch_mission(self):
        # download the vehicle waypoints
        print "fetching mission.."
        self.mission_cmds = self.vehicle.commands
        self.mission_cmds.download()
        self.mission_cmds.wait_ready()
        if not self.mission_cmds is None:
            print "retrieved mission with %d commands" % self.mission_cmds.count
        else:
            print "failed to retrieve mission"

    # check home - intermittently checks for changes to the home location
    def check_home(self):

        # return immediately if home has already been initialised
        if self.home_initialised:
            return True

        # check for home no more than once every two seconds
        if (time.time() - self.last_home_check > 2):

            # update that we have performed a status check
            self.last_home_check = time.time()

            # check if we have a vehicle
            if self.vehicle is None:
                self.vehicle = self.get_vehicle_with_dronekit()
                return

            # download the vehicle waypoints (and home) if we don't have them already
            if self.mission_cmds is None:
                self.fetch_mission()
                return False

            # ensure the vehicle's position is known
            if self.vehicle.location.global_relative_frame is None:
                print "waiting for vehicle position.."
                return False
            if self.vehicle.location.global_relative_frame.lat is None or self.vehicle.location.global_relative_frame.lon is None or self.vehicle.location.global_relative_frame.alt is None:
                print "waiting for vehicle position.."
                return False

            # get home location from mission command list
            if self.vehicle.home_location is None:
                print "waiting for home location.."
                self.fetch_mission()
                return False

            # sanity check home position
            if self.vehicle.home_location.lat == 0 and self.vehicle.home_location.lon == 0:
                print "home position all zero, re-fetching.."
                self.fetch_mission()
                return False

            # set home position
            PositionVector.set_home_location(LocationGlobal(self.vehicle.home_location.lat,self.vehicle.home_location.lon,0))
            self.home_initialised = True
            print "got home location"

            # To-Do: if we wish to have the same home position as the flight controller
            # we must download the home waypoint again whenever the vehicle is armed

        # return whether home has been initialised or not
        return self.home_initialised

    # checks if video output should be started
    def check_video_out(self):

        # return immediately if video has already been started
        if not self.writer is None:
            return
        #self.writer = balloon_video.open_video_writer()
        # start video once vehicle is armed
        if self.vehicle.armed and self.arm:
            sc_logger.text(sc_logger.GENERAL, "armed")
            self.arm = False
            #self.writer = balloon_video.open_video_writer()
        if not self.vehicle.armed and not self.arm:
            sc_logger.text(sc_logger.GENERAL, "disarm")
            self.arm = True

    # buat trigger payload, disini pake relay
    def servo(self, sv):
         #input the message
         msg = self.vehicle.message_factory.command_long_encode(0, 0, # target system, target component
                                                     mavutil.mavlink.MAV_CMD_DO_SET_RELAY,
                                                     0, # konfirmasi
                                                     3, # pin relay pada AUX OUT 3
                                                     sv, # 1 = ON, 0 = OFF
                                                     0, 0, 0, 0, 0) # param 1 ~ 5 ga dipake
         # send command to vehicle
         self.vehicle.send_mavlink(msg)
         self.vehicle.flush()
		 
    # buat trigger payload, disini pake relay
    def relay(self, st):
         #input the message
         msg = self.vehicle.message_factory.command_long_encode(0, 0, # target system, target component
                                                     mavutil.mavlink.MAV_CMD_DO_SET_RELAY,
                                                     0, # konfirmasi
                                                     0, # pin 0 relay pada AUX OUT 5
                                                     st, # 1 = ON, 0 = OFF
                                                     0, 0, 0, 0, 0) # param 1 ~ 5 ga dipake
         # send command to vehicle
         self.vehicle.send_mavlink(msg)
         self.vehicle.flush()

    # buat trigger payload, disini pake relay
    def relay1(self, st1):
         #input the message
         msg = self.vehicle.message_factory.command_long_encode(0, 0, # target system, target component
                                                     mavutil.mavlink.MAV_CMD_DO_SET_RELAY,
                                                     0, # konfirmasi
                                                     1, # pin 1 relay pada AUX OUT 6
                                                     st1, # 1 = ON, 0 = OFF
                                                     0, 0, 0, 0, 0) # param 1 ~ 5 ga dipake
         # send command to vehicle
         self.vehicle.send_mavlink(msg)
         self.vehicle.flush()

    # buat trigger payload, disini pake relay
    def relay2(self, st2):
         #input the message
         msg = self.vehicle.message_factory.command_long_encode(0, 0, # target system, target component
                                                     mavutil.mavlink.MAV_CMD_DO_SET_RELAY,
                                                     0, # konfirmasi
                                                     2, # pin 2 relay pada AUX OUT 4
                                                     st2, # 1 = ON, 0 = OFF
                                                     0, 0, 0, 0, 0) # param 1 ~ 5 ga dipake
         # send command to vehicle
         self.vehicle.send_mavlink(msg)
         self.vehicle.flush()
    
    # check_status - poles vehicle' status to determine if we are in control of vehicle or not
    def check_status(self):

        # Reset lanjut_cmd
        if (self.vehicle.mode.name == "LOITER" or self.vehicle.mode.name == "STABILIZE") and self.lanjut_cmd > 1:
            sc_logger.text(sc_logger.GENERAL, "Resetting Mission")
            self.c = False
            self.drop = 0
            self.relay(0)
            self.lanjut_cmd = 1

        # download the vehicle waypoints if we don't have them already
        # To-Do: do not load waypoints if vehicle is armed
        if self.mission_cmds is None:
            self.fetch_mission()
            return

        # Check for Auto mode and executing Nav-Guided command
        if self.vehicle.mode.name == "AUTO":
            self.relay1(0) #lampu indikator off
            # get active command number
            active_command = self.vehicle.commands.next
            #print "Active Command dan Lanjut Command adalah %s, %s" % (active_command,self.lanjut_cmd)
            sc_logger.text(sc_logger.GENERAL, "Active dan Lanjut Command: %s, %s" % (active_command, self.lanjut_cmd))

            # Buat ubah ke GUIDED
            if active_command == 1 and self.lanjut_cmd == 1:
                sc_logger.text(sc_logger.GENERAL, "Taking Off!!!")
                self.lanjut_cmd = 3
            if (active_command < self.vehicle.commands.count) and (active_command == self.lanjut_cmd): # WP cek api
                self.lanjut_cmd += 1
                sc_logger.text(sc_logger.GENERAL, "ON TARGET, ubah Mode ke GUIDED")
                self.c = True
                self.a = True
                self.waktu1 = 0
                self.vehicle.mode = VehicleMode("GUIDED")
                self.vehicle.flush()
            if active_command == self.vehicle.commands.count:
                sc_logger.text(sc_logger.GENERAL, "Misi selesai, Mode RTL dan LAND")
                self.vehicle.mode = VehicleMode("RTL")
                self.vehicle.flush()

        # we are active in guided mode
        if self.vehicle.mode.name == "GUIDED":
            if not self.controlling_vehicle:
                self.controlling_vehicle = True
                print "taking control of vehicle in GUIDED"
                print "Mulai cari Target"
                sc_logger.text(sc_logger.GENERAL, "taking control of vehicle in GUIDED dan mulai cari Target")
            return
    
        # if we got here then we are not in control
        if self.controlling_vehicle:
            self.controlling_vehicle = False
            print "giving up control of vehicle in %s" % self.vehicle.mode.name 

    # condition_yaw - send condition_yaw mavlink command to vehicle so it points at specified heading (in degrees)
    def condition_yaw(self, heading):
        # create the CONDITION_YAW command
        msg = self.vehicle.message_factory.mission_item_encode(0, 0,  # target system, target component
                                                     0,     # sequence
                                                     mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame
                                                     mavutil.mavlink.MAV_CMD_CONDITION_YAW,         # command
                                                     2, # current - set to 2 to make it a guided command
                                                     0, # auto continue
                                                     heading, 0, 0, 0, 0, 0, 0) # param 1 ~ 7
        # send command to vehicle
        self.vehicle.send_mavlink(msg)
        self.vehicle.flush()

    # send_nav_velocity - send nav_velocity command to vehicle to request it fly in specified direction
    def send_nav_velocity(self, velocity_x, velocity_y, velocity_z):
        # create the SET_POSITION_TARGET_LOCAL_NED command
        msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
                                                     0,       # time_boot_ms (not used)
                                                     0, 0,    # target system, target component
                                                     mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
                                                     0b0000111111000111, # type_mask (only speeds enabled)
                                                     0, 0, 0, # x, y, z positions (not used)
                                                     velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s
                                                     0, 0, 0, # x, y, z acceleration (not used)
                                                     0, 0)    # yaw, yaw_rate (not used) 
        # send command to vehicle
        self.vehicle.send_mavlink(msg)
        self.vehicle.flush()

    # advance_current_cmd - ask vehicle to advance to next command (i.e. abort current command)
    def advance_current_cmd(self):

        # exit immediately if we are not in AUTO mode or not controlling the vehicle
        if not self.vehicle.mode.name == "AUTO" or not self.controlling_vehicle:
            return

        # download the vehicle waypoints if we don't have them already
        if self.mission_cmds is None:
            self.fetch_mission()

        # get active command
        active_command = self.vehicle.commands.next

        # ensure there is one more command at least
        if (self.vehicle.commands.count > active_command):
            # create the MISSION_SET_CURRENT command
            msg = self.vehicle.message_factory.mission_set_current_encode(0, 0, active_command+1) # target system, target component, sequence
            # send command to vehicle
            self.vehicle.send_mavlink(msg)
            self.vehicle.flush()
        else:
            print "Failed to advance command"

    # get_frame - get a single frame from the camera or simulator
    def get_frame(self):
        if self.use_simulator:
            veh_pos = PositionVector.get_from_location(self.vehicle.location.global_relative_frame)
            frame = balloon_sim.get_simulated_frame(veh_pos, self.vehicle.attitude.roll, self.vehicle.attitude.pitch, self.vehicle.attitude.yaw)
        else:
            frame = balloon_video.get_image()
        return frame

    # get image from balloon_video class and look for balloon, results are held in the following variables:
    #    self.balloon_found : set to True if balloon is found, False otherwise
    #    self.balloon_pitch : earth frame pitch (in radians) from vehicle to balloon (i.e. takes account of vehicle attitude)
    #    self.balloon_heading : earth frame heading (in radians) from vehicle to balloon
    #    self.balloon_distance : distance (in meters) from vehicle to balloon
    #    self.balloon_pos : position vector of balloon's position as an offset from home in meters

    def output_frame_rate_stats(self):
        # get current time
        now = time.time()

        # output stats each 10seconds
        time_diff = now - self.stats_start_time 
        if time_diff < 10 or time_diff <= 0:
            return

        # output frame rate
        frame_rate = self.num_frames_analysed / time_diff
        print "FrameRate: %f (%d frames in %f seconds)" % (frame_rate, self.num_frames_analysed, time_diff)

        # reset stats
        self.num_frames_analysed = 0
        self.stats_start_time = now


    

    ### FROM PRECSION LAND !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    
    def analisis_image(self):
        # record time
        now = time.time()
        #PositionVectors = PositionVector()
        # capture vehicle position
        self.vehicle_pos = PositionVector.get_from_location(self.vehicle.location.global_relative_frame)

        # capture vehicle attitude in buffer
        self.att_hist.update()

        # get delayed attitude from buffer
        veh_att_delayed = self.att_hist.get_attitude(now - self.attitude_delay)

        # get new image from camera
        f = self.get_frame()
        #create an image processor
        if self.mission_on_guide_mode == 4:
            self.fire_found, xpos, ypos, size = fire_finder.filter(f)
            #result = detector.analyze_frame(f)
            self.attitude = self.get_attitude()
            if self.fire_found == True:
                self.b = 0
                sc_logger.text(sc_logger.GENERAL, "Target terdeteksi!")
                print xpos,ypos,size
                if self.vehicle.mode.name == "GUIDED":
                    sc_logger.text(sc_logger.GENERAL, "Target terdeteksi, mendekati target!")
                self.move_to_target_fire(xpos,ypos,size,self.attitude,self.vehicle_pos)
                self.relay1(1) #lampu indikator on
                #self.servo(0)#test untuk servo kamera
            elif self.vehicle.mode.name == "GUIDED":
                self.relay1(0) #lampu indikator off
                #self.servo(1)#test untuk servo kamera
                self.b += 1
                if self.b > 25: #15
                    self.b = 0
                    #print "Api tidak terdeteksi, ubah mode ke AUTO"
                    #self.lanjut_cmd += 1
                    sc_logger.text(sc_logger.GENERAL, "Target tidak terdeteksi, ubah mode ke AUTO")
                    self.a = False
                    self.waktu1 = 0
                    #self.c = True
                    self.controlling_vehicle = False
                    self.vehicle.mode = VehicleMode("AUTO")
                    self.vehicle.flush()
            #rend_Image = detector.add_target_highlights(f, result[3])
            sc_logger.image(sc_logger.GUI, f)

            if not self.writer is None:
            # save image for debugging later
                self.writer.write(f)

            # increment frames analysed for stats
            self.num_frames_analysed += 1
            if self.stats_start_time == 0:
                self.stats_start_time = time.time()

    #get_attitude - returns pitch, roll, and yaw of vehicle
    def get_attitude(self):
        return self.vehicle.attitude

    #initialize_landing - reset the state machine which controls the flow of the landing routine
    def initialize_landing(self):
        
        #how mant times we have attempted landing
        self.attempts = 0

        #Last time in millis since we had a valid target
        self.last_valid_target = 0

        #State variable climbing to scan for the target
        self.climbing = False

        #State variable which determines if this program will continue to run
        self.pl_enabled = True

        #State variable used to represent if autopilot is active
        self.initial_descent = True

        #State variable which represents a know target in landing area
        self.target_detected = False

    #move_to_target - fly aircraft to landing pad
    def move_to_target_fire(self,x,y,size,attitude,location):

        # exit immediately if we are not controlling the vehicle
        if not self.controlling_vehicle:
            return

        # get active command
        active_command = self.vehicle.commands.next

        # get current time
        now = time.time()

        # exit immediately if it's been too soon since the last update
        if (now - self.guided_last_update) < self.vel_update_rate:
            return;

        # if we have a new balloon position recalculate velocity vector
        if (self.fire_found):
            #x,y = target_info[1]
            
            #pitch_dir,yaw_dir = balloon_finder.pixels_to_direction(x, y, attitude.roll, attitude.pitch, attitude.yaw)
            #yaw_dir = yaw_dir % 360 
            #print "Target Yaw:%f" %(yaw_dir)
            #target_distance = target_info[2]
            #print "Target Distance:%f" %(target_distance*100)
            #shift origin to center of image

            x,y = shift_to_origin((x,y),self.camera_width,self.camera_height)
            hfov = self.camera_hfov
            vfov = self.camera_vfov

            #stabilize image with vehicle attitude
            x -= (self.camera_width / hfov) * math.degrees(attitude.roll)
            y += (self.camera_height / vfov) * math.degrees(attitude.pitch)


            #convert to distance
            X, Y = self.pixel_point_to_position_xy((x,y),location.z)

            #convert to world coordinates
            target_headings = math.atan2(Y,X) #% (2*math.pi)
            target_heading = (attitude.yaw - target_headings) 
            target_distance = math.sqrt(X**2 + Y**2)
         
            #sc_logger.text(sc_logger.GENERAL, "Distance to target: {0}".format(round(target_distance,2)))
        
            
            #calculate speed toward target
            speed = target_distance * self.dist_to_vel
            #apply max speed limit
            speed = min(speed,self.vel_speed_max)

            #calculate cartisian speed
            vx = speed * math.sin(target_heading) * -1.0
            vy = speed * math.cos(target_heading) #*-1.0

            print "Found Target go to vx:%f vy:%f Alt:%f target_distance:%f headings:%f heading:%f " % (vx,vy,location.z,target_distance,target_headings,target_heading)

            #only descend when on top of target
            if(location.z > 3.5):
                vz = 0.25
            else:
                vz = 0
					
            if active_command == 3: #PICK MP1
                #Jika ketinggian sudah dibawah 4 meter, maka MAGNET ON
                #self.servo(0)#kamera siap untuk pick
                if (location.z < 4.0):
                    if(location.z > 1.6):
                        vz = 0.2
                    else:
                        vz = 0
                        # if (location.z > 1.60): #Menurunkan Kecepatan Descent
                        #     vz = 0.2
                        # else:
                        #     vz = 0
                        if (self.c == True):
                            self.relay2(1) #magnet ON 
                            self.waktu = 0
                            self.c = False
                    if (location.z < 1.7): #if (GPIO.20 == HIGH):
                        print ("Payload sudah diambil, lanjut misi")
                        sc_logger.text(sc_logger.GENERAL, "Payload sudah diambil, lanjut misi")
                        self.controlling_vehicle = False
                        self.vehicle.mode = VehicleMode("AUTO")
                        self.vehicle.flush()

            # if active_command = 3:
            #     if
						
            if active_command == 4: #DROP MP1
                #self.servo(1)#kamera siap untuk drop
                if(location.z > 2.0):
                    vz = 0.25
                else:
                    if (location.z > 2.5): #Menurunkan Kecepatan Descent
                         vz = 0.2
                    else:
                         vz = 0
                         if (self.c == True):
                            self.relay2(0) #magnet OFF
                            self.waktu = 0
                            self.c = False
                            self.drop = 1
                            print ("Payload sudah DROP, lanjut misi")
                            sc_logger.text(sc_logger.GENERAL, "Payload sudah DROP, lanjut misi")
                            self.vehicle.commands.next = 5
                            self.controlling_vehicle = False
                            self.vehicle.mode = VehicleMode("AUTO")
                            self.vehicle.flush()
                        
            if active_command == 5: #DROP MP1 atau MP2  
                #self.servo(1)#kamera siap untuk drop
                if(location.z > 2.0):
                    vz = 0.25
                else:
                    if (location.z > 2.5): #Menurunkan Kecepatan Descent
                         vz = 0.2
                    else:
                         vz = 0
                         if (self.c == True):
                            self.relay2(0) #magnet OFF
                            self.waktu = 0
                            self.c = False
                            self.drop = 2
                            print ("Payload sudah DROP, lanjut misi")
                            sc_logger.text(sc_logger.GENERAL, "Payload sudah DROP, lanjut misi")
                            self.controlling_vehicle = False
                            self.vehicle.mode = VehicleMode("AUTO")
                            self.vehicle.flush() 
                        
            if active_command == 6: #DROP LOG
                #self.servo(1)#kamera siap untuk drop
                if(location.z > 2.5):
                    vz = 0.25
                else:
                    if (location.z > 3.5): #Menurunkan Kecepatan Descent
                         vz = 0.2
                    else:
                         vz = 0
                         if (self.c == True):
                            self.relay(1) #buka payload
                            self.waktu = 0
                            self.c = False
                            print ("Payload sudah DROP, lanjut misi")
                            sc_logger.text(sc_logger.GENERAL, "Payload sudah DROP, lanjut misi")
                            self.controlling_vehicle = False
                            self.vehicle.mode = VehicleMode("AUTO")
                            self.vehicle.flush()  	
                            
            if active_command == 7: #PICK MP1
                #Jika ketinggian sudah dibawah 4 meter, maka MAGNET ON
                #self.servo(0)#kamera siap untuk pick
                if (location.z < 4.0):
                    if(location.z > 1.6):
                        vz = 0.2
                    else:
                        vz = 0
                        # if (location.z > 1.60): #Menurunkan Kecepatan Descent
                        #     vz = 0.2
                        # else:
                        #     vz = 0
                        if (self.c == True):
                            self.relay2(1) #magnet ON 
                            self.waktu = 0
                            self.c = False
                    if (location.z < 1.7): #if (GPIO.20 == HIGH):
                        print ("Payload sudah diambil, lanjut misi")
                        sc_logger.text(sc_logger.GENERAL, "Payload sudah diambil, lanjut misi")
                        self.controlling_vehicle = False
                        if (self.drop == 2):
                            self.vehicle.commands.next = 8
                            self.lanjut_cmd += 1
                        self.vehicle.mode = VehicleMode("AUTO")
                        self.vehicle.flush()
                                                
            if active_command == 9: #DROP MP2  
                #self.servo(1)#kamera siap untuk drop
                if(location.z > 2.0):
                    vz = 0.25
                else:
                    if (location.z > 2.5): #Menurunkan Kecepatan Descent
                         vz = 0.2
                    else:
                         vz = 0
                         if (self.c == True):
                            self.relay2(0) #magnet OFF
                            self.waktu = 0
                            self.c = False
                            print ("Payload sudah DROP, lanjut misi")
                            sc_logger.text(sc_logger.GENERAL, "Payload sudah DROP, lanjut misi")
                            self.controlling_vehicle = False
                            self.vehicle.mode = VehicleMode("AUTO")
                            self.vehicle.flush()  

            #send velocity commands  toward target heading
            self.send_nav_velocity(vx,vy,vz)

    #move_to_target - fly aircraft to landing pad
    def move_to_target_land(self,target_info,attitude,location):

        x,y = target_info[1]
        
        #pitch_dir,yaw_dir = balloon_finder.pixels_to_direction(x, y, attitude.roll, attitude.pitch, attitude.yaw)
        #yaw_dir = yaw_dir % 360 
        #print "Target Yaw:%f" %(yaw_dir)
        #target_distance = target_info[2]
        #print "Target Distance:%f" %(target_distance*100)
        #shift origin to center of image

        x,y = shift_to_origin((x,y),self.camera_width,self.camera_height)
        hfov = self.camera_hfov
        vfov = self.camera_vfov

        #stabilize image with vehicle attitude
        x -= (self.camera_width / hfov) * math.degrees(attitude.roll)
        y += (self.camera_height / vfov) * math.degrees(attitude.pitch)


        #convert to distance
        X, Y = self.pixel_point_to_position_xy((x,y),location.z)

        #convert to world coordinates
        target_headings = math.atan2(Y,X) #% (2*math.pi)
        target_heading = (attitude.yaw - target_headings) 
        target_distance = math.sqrt(X**2 + Y**2)
     
        sc_logger.text(sc_logger.GENERAL, "Distance to target: {0}".format(round(target_distance,2)))
    
        
        #calculate speed toward target
        speed = target_distance * self.dist_to_vel
        #apply max speed limit
        speed = min(speed,self.vel_speed_max)

        #calculate cartisian speed
        vx = speed * math.sin(target_heading)*  -1.0
        vy = speed * math.cos(target_heading) #* -1.0

        print "Found Target go to vx:%f vy:%f Alt:%f target_distance:%f" % (vx,vy,location.z,target_distance)

        #only descend when on top of target
        if(target_distance < self.descent_radius):
            vz = 0
        else:
            vz = 0

        #send velocity commands toward target heading
        self.send_nav_velocity(vx,vy,vz)

    #move_to_target - fly aircraft to landing pad
    def move_to_target(self,target_info,attitude,location):

        x,y = target_info[1]
        
        #pitch_dir,yaw_dir = balloon_finder.pixels_to_direction(x, y, attitude.roll, attitude.pitch, attitude.yaw)
        #yaw_dir = yaw_dir % 360 
        #print "Target Yaw:%f" %(yaw_dir)
        #target_distance = target_info[2]
        #print "Target Distance:%f" %(target_distance*100)
        #shift origin to center of image

        x,y = shift_to_origin((x,y),self.camera_width,self.camera_height)
        hfov = self.camera_hfov
        vfov = self.camera_vfov

        #stabilize image with vehicle attitude
        x -= (self.camera_width / hfov) * math.degrees(attitude.roll)
        y += (self.camera_height / vfov) * math.degrees(attitude.pitch)


        #convert to distance
        X, Y = self.pixel_point_to_position_xy((x,y),location.z)

        #convert to world coordinates
        target_headings = math.atan2(Y,X) #% (2*math.pi)
        target_heading = (attitude.yaw - target_headings) 
        target_distance = math.sqrt(X**2 + Y**2)
     
        sc_logger.text(sc_logger.GENERAL, "Distance to target: {0}".format(round(target_distance,2)))
    
        
        #calculate speed toward target
        speed = target_distance * self.dist_to_vel
        #apply max speed limit
        speed = min(speed,self.vel_speed_max)

        #calculate cartisian speed
        vx = speed * math.sin(target_heading) * -1.0
        vy = speed * math.cos(target_heading) 

        print "Found Target go to vx:%f vy:%f Alt:%f target_distance:%f" % (vx,vy,location.z,target_distance)

        #only descend when on top of target
        if(target_distance > self.descent_radius):
            vz = 0
        else:
            vz = self.descent_rate

        #send velocity commands toward target heading
        self.send_nav_velocity(vx,vy,vz)

	#move_to_target - fly aircraft to landing pad
    def move_to_target_cam_forward(self,target_info,attitude,location):
        x,y = target_info[1]
        pitch_dir,yaw_dir = balloon_finder.pixels_to_direction(x, y, attitude.roll, attitude.pitch, attitude.yaw)
        yaw_dir = yaw_dir % 360 
        print "Target Yaw:%f" %(yaw_dir)
        target_distance = target_info[2]*10
        print "Target Distance:%f" %(target_distance)
        #shift origin to center of image
      
        print "Found Target at yaw:%f heading:%f Alt:%f Dist:%f" % (attitude.yaw,yaw_dir,location.z,target_distance)

        sc_logger.text(sc_logger.GENERAL, "Distance to target: {0}".format(round(target_distance,2)))

        print "mode detection"
        #self.condition_yaw(math.fabs(math.degrees(target_heading)))
        if(target_distance > 4):
            vx = 0
            vy = 0 
            vz = 0
        else:
            vx = 0
            vy = 0
            vz = 0
        #send velocity commands toward target heading
        self.send_nav_velocity(vx,vy,vz)
        self.condition_yaw(yaw_dir)

    #inside_landing_area - determine is we are in a landing zone 0 = False, 1 = True, -1 = below the zone
    def inside_landing_area(self):

        vehPos = PositionVector.get_from_location(self.vehicle.location.global_relative_frame)
        landPos = PositionVector.get_from_location(PositionVector.get_home_location())
        '''
        vehPos = PositionVector.get_from_location(Location(0,0,10))
        landPos = PositionVector.get_from_location(Location(0,0,0))
        '''
        if(PositionVector.get_distance_xy(vehPos,landPos) < self.landing_area_radius):
            #below area
            if(vehPos.z < self.landing_area_min_alt):
                print "VehPos.Z: %f " % (vehPos.z)
                return 1
            #in area
            else:
                return 1
        #outside area
        else:
            return 0

    #pixel_point_to_position_xy - convert position in pixels to position in meters
    #pixel_position - distance in pixel from CENTER of image
    #distance- distance from the camera to the object in  meters 
    def pixel_point_to_position_xy(self,pixel_position,distance):
        thetaX = pixel_position[0] * self.camera_hfov / self.camera_width
        thetaY = pixel_position[1] * self.camera_vfov / self.camera_height
        x = distance * math.tan(math.radians(thetaX))
        y = distance * math.tan(math.radians(thetaY))

        return (x,y)

    def run(self):
        while True:

            now = time.time()

            # only process images once home has been initialised
            if self.check_home():

                # start video if required
                self.check_video_out()
    
                # check if we are controlling the vehicle
                self.check_status()

                if self.mission_on_guide_mode == 4:
                    self.analisis_image()

                # output stats
                self.output_frame_rate_stats()

            # Don't suck up too much CPU, only process a new image occasionally
            time.sleep(0.05)

        if not self.use_simulator:
            balloon_video.stop_background_capture()

    # complete - balloon strategy has somehow completed so return control to the autopilot
    def complete(self):
        # debug
        if self.debug:
            print "Complete!"

        # stop the vehicle and give up control
        if self.controlling_vehicle:
            self.guided_target_vel = (0,0,0)
            self.send_nav_velocity(self.guided_target_vel[0], self.guided_target_vel[1], self.guided_target_vel[2])
            self.guided_last_update = time.time()

        # if in GUIDED mode switch back to LOITER
        if self.vehicle.mode.name == "GUIDED":
            self.vehicle.mode = VehicleMode("LOITER")
            self.vehicle.flush()

        # if in AUTO move to next command
        if self.vehicle.mode.name == "AUTO":
            self.advance_current_cmd();

        # flag we are not in control of the vehicle
        self.controlling_vehicle = False
            
        return
class BalloonStrategy(object):
    def __init__(self):

        # First get an instance of the API endpoint (the connect via web case will be similar)
        self.api = local_connect()

        # Our vehicle (we assume the user is trying to control the virst vehicle attached to the GCS)
        self.vehicle = self.api.get_vehicles()[0]

        # initialised flag
        self.home_initialised = False
        # timer to intermittently check for home position
        self.last_home_check = time.time()

        # historical attitude
        self.att_hist = AttitudeHistory(self.vehicle, 2.0)
        self.attitude_delay = 0.0               # expected delay between image and attitude

        # search variables
        self.search_state = 0                   # 0 = not search, 1 = spinning and looking, 2 = rotating to best balloon and double checking, 3 = finalise yaw 
        self.search_start_heading = None        # initial heading of vehicle when search began
        self.search_target_heading = None       # the vehicle's current target heading (updated as vehicle spins during search)
        self.search_heading_change = None       # heading change (in radians) performed so far during search
        self.search_balloon_pos = None          # position (as an offset from home) of closest balloon (so far) during search
        self.search_balloon_heading = None      # earth-frame heading (in radians) from vehicle to closest balloon
        self.search_balloon_pitch_top = None        # earth-frame pitch (in radians) from vehicle to closest balloon
        self.search_balloon_distance = None     # distance (in meters) from vehicle to closest balloon
        self.targeting_start_time = 0           # time vehicle first pointed towards final target (used with delay_time below)
        self.targeting_delay_time = balloon_config.config.get_float('general','SEARCH_TARGET_DELAY',2.0)    # time vehicle waits after pointing towards ballon and before heading towards it
        self.search_yaw_speed = balloon_config.config.get_float('general','SEARCH_YAW_SPEED',5.0) 

        # vehicle mission
        self.mission_cmds = None
        self.mission_alt_min = 0                # min altitude from NAV_GUIDED mission command (we ignore balloons below this altitude).  "0" means no limit
        self.mission_alt_max = 0                # max altitude from NAV_GUIDED mission command (we ignore balloons above this altitude).  "0" means no limit
        self.mission_distance_max = 0           # max distance from NAV_GUIDED mission command (we ignore balloons further than this distance).  "0" means no limit

        # we are not in control of vehicle
        self.controlling_vehicle = False

        # vehicle position captured at time camera image was captured
        self.vehicle_pos = None

        # balloon direction and position estimate from latest call to analyse_image
        self.balloon_found = False
        self.balloon_pitch = None
        self.balloon_pitch_top = None           # earth-frame pitch (in radians) from vehicle to top of closest balloon
        self.balloon_heading = None
        self.balloon_distance = None
        self.balloon_pos = None             # last estimated position as an offset from home

        # time of the last target update sent to the flight controller
        self.guided_last_update = time.time()

        # latest velocity target sent to flight controller
        self.guided_target_vel = None

        # time the target balloon was last spotted
        self.last_spotted_time = 0

        # if we lose sight of a balloon for this many seconds we will consider it lost and give up on the search
        self.lost_sight_timeout = 3

        # The module only prints log messages unless the vehicle is in GUIDED mode (for testing).
        # Once this seems to work reasonablly well change self.debug to False and then it will
        # actually _enter_ guided mode when it thinks it sees a balloon
        self.debug = balloon_config.config.get_boolean('general','debug',True)

        # use the simulator to generate fake balloon images
        self.use_simulator = balloon_config.config.get_boolean('general','simulate',False)

        # start background image grabber
        if not self.use_simulator:
            balloon_video.start_background_capture()

        # initialise video writer
        self.writer = None

        # horizontal velocity pid controller.  maximum effect is 10 degree lean
        xy_p = balloon_config.config.get_float('general','VEL_XY_P',1.0)
        xy_i = balloon_config.config.get_float('general','VEL_XY_I',0.0)
        xy_d = balloon_config.config.get_float('general','VEL_XY_D',0.0)
        xy_imax = balloon_config.config.get_float('general','VEL_XY_IMAX',10.0)
        self.vel_xy_pid = pid.pid(xy_p, xy_i, xy_d, math.radians(xy_imax))

        # vertical velocity pid controller.  maximum effect is 10 degree lean
        z_p = balloon_config.config.get_float('general','VEL_Z_P',2.5)
        z_i = balloon_config.config.get_float('general','VEL_Z_I',0.0)
        z_d = balloon_config.config.get_float('general','VEL_Z_D',0.0)
        z_imax = balloon_config.config.get_float('general','VEL_IMAX',10.0)
        self.vel_z_pid = pid.pid(z_p, z_i, z_d, math.radians(z_imax))

        # velocity controller min and max speed
        self.vel_speed_min = balloon_config.config.get_float('general','VEL_SPEED_MIN',1.0)
        self.vel_speed_max = balloon_config.config.get_float('general','VEL_SPEED_MAX',4.0)
        self.vel_speed_last = 0.0   # last recorded speed
        self.vel_accel = balloon_config.config.get_float('general','VEL_ACCEL', 0.5)    # maximum acceleration in m/s/s
        self.vel_dist_ratio = balloon_config.config.get_float('general','VEL_DIST_RATIO', 0.5) 

        # pitch angle to hit balloon at.  negative means come down from above
        self.vel_pitch_target = math.radians(balloon_config.config.get_float('general','VEL_PITCH_TARGET',-5.0))

        # velocity controller update rate
        self.vel_update_rate = balloon_config.config.get_float('general','VEL_UPDATE_RATE_SEC',0.2)

    # fetch_mission - fetch mission from flight controller
    def fetch_mission(self):
        # download the vehicle waypoints
        self.mission_cmds = self.vehicle.commands
        self.mission_cmds.download()
        self.mission_cmds.wait_valid()

    # check home - intermittently checks for changes to the home location
    def check_home(self):

        # return immediately if home has already been initialised
        if self.home_initialised:
            return True

        # check for home no more than once every two seconds
        if (time.time() - self.last_home_check > 2):

            # update that we have performed a status check
            self.last_home_check = time.time()

            # check if we have a vehicle
            if self.vehicle is None:
                self.vehicle = self.api.get_vehicles()[0]
                return

            # ensure the vehicle's position is known
            if self.vehicle.location is None:
                return False
            if self.vehicle.location.lat is None or self.vehicle.location.lon is None or self.vehicle.location.alt is None:
                return False

            # download the vehicle waypoints if we don't have them already
            if self.mission_cmds is None:
                self.fetch_mission()
                return False

            # get the home lat and lon
            home_lat = self.mission_cmds[0].x
            home_lon = self.mission_cmds[0].y

            # sanity check the home position
            if home_lat is None or home_lon is None:
                return False

            # sanity check again and set home position
            if home_lat <> 0 and home_lon <> 0:
                PositionVector.set_home_location(Location(home_lat,home_lon,0))
                self.home_initialised = True
            else:
                self.mission_cmds = None

            # To-Do: if we wish to have the same home position as the flight controller
            # we must download the home waypoint again whenever the vehicle is armed

        # return whether home has been initialised or not
        return self.home_initialised

    # checks if video output should be started
    def check_video_out(self):

        # return immediately if video has already been started
        if not self.writer is None:
            return

        # start video once vehicle is armed
        if self.vehicle.armed:
            self.writer = balloon_video.open_video_writer()

    # check_status - poles vehicle' status to determine if we are in control of vehicle or not
    def check_status(self):

        # we are active in guided mode
        if self.vehicle.mode.name == "GUIDED":
            if not self.controlling_vehicle:
                self.controlling_vehicle = True
                # clear out any limits on balloon position
                self.mission_alt_min = 1
                self.mission_alt_max = 0
                self.mission_distance_max = 50
                # start search for balloon
                self.start_search()
            return

        # download the vehicle waypoints if we don't have them already
        # To-Do: do not load waypoints if vehicle is armed
        if self.mission_cmds is None:
            self.fetch_mission()
            return

        # Check for Auto mode and executing Nav-Guided command
        if self.vehicle.mode.name == "AUTO":

            # get active command number and mavlink id
            active_command = self.vehicle.commands.next
            active_command_id = self.vehicle.commands[active_command].command

            # ninety two is the MAVLink id for Nav-Guided-Enable commands
            if active_command_id == 92:
                if not self.controlling_vehicle:
                    self.controlling_vehicle = True
                    self.mission_alt_min = self.vehicle.commands[active_command].param2
                    self.mission_alt_max = self.vehicle.commands[active_command].param3
                    self.mission_distance_max = self.vehicle.commands[active_command].param4
                    self.start_search()
                return    
    
        # if we got here then we are not in control
        self.controlling_vehicle = False

    # condition_yaw - send condition_yaw mavlink command to vehicle so it points at specified heading (in degrees)
    def condition_yaw(self, heading):
        # create the CONDITION_YAW command
        msg = self.vehicle.message_factory.mission_item_encode(0, 0,  # target system, target component
                                                     0,     # sequence
                                                     mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame
                                                     mavutil.mavlink.MAV_CMD_CONDITION_YAW,         # command
                                                     2, # current - set to 2 to make it a guided command
                                                     0, # auto continue
                                                     heading, 0, 0, 0, 0, 0, 0) # param 1 ~ 7
        # send command to vehicle
        self.vehicle.send_mavlink(msg)
        self.vehicle.flush()

    # send_nav_velocity - send nav_velocity command to vehicle to request it fly in specified direction
    def send_nav_velocity(self, velocity_x, velocity_y, velocity_z):
        # create the SET_POSITION_TARGET_LOCAL_NED command
        msg = self.vehicle.message_factory.set_position_target_local_ned_encode(
                                                     0,       # time_boot_ms (not used)
                                                     0, 0,    # target system, target component
                                                     mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame
                                                     0,       # type_mask (not used)
                                                     0, 0, 0, # x, y, z positions (not used)
                                                     velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s
                                                     0, 0, 0, # x, y, z acceleration (not used)
                                                     0, 0)    # yaw, yaw_rate (not used) 
        # send command to vehicle
        self.vehicle.send_mavlink(msg)
        self.vehicle.flush()

    # advance_current_cmd - ask vehicle to advance to next command (i.e. abort current command)
    def advance_current_cmd(self):

        # exit immediately if we are not in AUTO mode or not controlling the vehicle
        if not self.vehicle.mode.name == "AUTO" or not self.controlling_vehicle:
            return

        # download the vehicle waypoints if we don't have them already
        if self.mission_cmds is None:
            self.fetch_mission()

        # get active command
        active_command = self.vehicle.commands.next

        # ensure there is one more command at least
        if (self.vehicle.commands.count > active_command):
            # create the MISSION_SET_CURRENT command
            msg = self.vehicle.message_factory.mission_set_current_encode(0, 0, active_command+1) # target system, target component, sequence
            # send command to vehicle
            self.vehicle.send_mavlink(msg)
            self.vehicle.flush()
        else:
            print "Failed to advance command"

    # get_frame - get a single frame from the camera or simulator
    def get_frame(self):
        if self.use_simulator:
            veh_pos = PositionVector.get_from_location(self.vehicle.location)
            frame = balloon_sim.get_simulated_frame(veh_pos, self.vehicle.attitude.roll, self.vehicle.attitude.pitch, self.vehicle.attitude.yaw)
        else:
            frame = balloon_video.get_image()
        return frame

    # get image from balloon_video class and look for balloon, results are held in the following variables:
    #    self.balloon_found : set to True if balloon is found, False otherwise
    #    self.balloon_pitch : earth frame pitch (in radians) from vehicle to balloon (i.e. takes account of vehicle attitude)
    #    self.balloon_heading : earth frame heading (in radians) from vehicle to balloon
    #    self.balloon_distance : distance (in meters) from vehicle to balloon
    #    self.balloon_pos : position vector of balloon's position as an offset from home in meters
    def analyze_image(self):

        # record time
        now = time.time()

        # capture vehicle position
        self.vehicle_pos = PositionVector.get_from_location(self.vehicle.location)

        # capture vehicle attitude in buffer
        self.att_hist.update()

        # get delayed attitude from buffer
        veh_att_delayed = self.att_hist.get_attitude(now - self.attitude_delay)

        # get new image from camera
        f = self.get_frame()

        # look for balloon in image using blob detector        
        self.balloon_found, xpos, ypos, size = balloon_finder.analyse_frame(f)

        # add artificial horizon
        balloon_finder.add_artificial_horizon(f, veh_att_delayed.roll, veh_att_delayed.pitch)

        if self.balloon_found:
            # record time balloon was found
            self.last_spotted_time = now
        
            # convert x, y position to pitch and yaw direction (in radians)
            self.balloon_pitch, self.balloon_heading = balloon_finder.pixels_to_direction(xpos, ypos, veh_att_delayed.roll, veh_att_delayed.pitch, veh_att_delayed.yaw)
            self.balloon_pitch = math.radians(self.balloon_pitch)
            self.balloon_pitch_top = self.balloon_pitch + balloon_video.pixels_to_angle_y(size)  # add balloon radius so we aim for top of balloon
            self.balloon_heading = math.radians(self.balloon_heading)

            # get distance
            self.balloon_distance = get_distance_from_pixels(size, balloon_finder.balloon_radius_expected)

            # updated estimated balloon position
            self.balloon_pos = balloon_finder.project_position(self.vehicle_pos, self.balloon_pitch, self.balloon_heading, self.balloon_distance)

        # save image for debugging later
        if not self.writer is None:
            self.writer.write(f)

    # start_search - start search for balloon
    def start_search(self):
        # exit immediately if we are not controlling the vehicle
        if not self.controlling_vehicle:
            return
        # initialise search variables
        self.search_state = 1       # 0 = not search, 1 = spinning and looking, 2 = rotating to best balloon and double checking, 3 = finalise yaw
        self.search_balloon_pos = None
        self.search_start_heading = self.vehicle.attitude.yaw
        self.search_target_heading = self.search_start_heading
        self.search_total_angle = 0
        self.targeting_start_time = 0
        # reset vehicle speed
        self.vel_speed_last = 0

    # search - spin vehicle looking for balloon
    # analyze_image should have been called just before and should have filled in self.balloon_found, self.balloon_pos, balloon_distance, balloon_pitch and balloon_yaw
    def search_for_balloon(self):

        # exit immediately if we are not controlling the vehicle or do not know our position
        if not self.controlling_vehicle or self.vehicle_pos is None:
            return

        # exit immediately if we are not searching
        if self.search_state <= 0:
            return

        # search states: 0 = not searching, 1 = spinning and looking, 2 = rotating to best balloon and double checking, 3 = finalise yaw

        # search_state = 1: spinning and looking
        if (self.search_state == 1):

            # check if we have seen a balloon
            if self.balloon_found:
                # if this is the first balloon we've found or the closest, store it's position as the closest
                if (self.search_balloon_pos is None) or (self.balloon_distance < self.search_balloon_distance):
                    # check distance is within acceptable limits
                    if (self.mission_alt_min == 0 or self.balloon_pos.z >= self.mission_alt_min) and (self.mission_alt_max == 0 or self.balloon_pos.z <= self.mission_alt_max) and (self.mission_distance_max == 0 or self.balloon_distance <= self.mission_distance_max):
                        # record this balloon as the closest
                        self.search_balloon_pos = self.balloon_pos
                        self.search_balloon_heading = self.balloon_heading
                        self.search_balloon_pitch_top = self.balloon_pitch_top  # we target top of balloon
                        self.search_balloon_distance = self.balloon_distance
                        print "Found Balloon at heading:%f Alt:%f Dist:%f" % (math.degrees(self.balloon_heading), self.balloon_pos.z, self.balloon_distance)
                    else:
                        print "Balloon Ignored at heading:%f Alt:%f Dist:%f" % (math.degrees(self.balloon_heading), self.balloon_pos.z, self.balloon_distance)

            # update yaw so we keep spinning
            if math.fabs(wrap_PI(self.vehicle.attitude.yaw - self.search_target_heading)) < math.radians(self.search_yaw_speed * 2.0):
                # increase yaw target
                self.search_target_heading = self.search_target_heading - math.radians(self.search_yaw_speed)
                self.search_total_angle = self.search_total_angle + math.radians(self.search_yaw_speed)
                # send yaw heading
                self.condition_yaw(math.degrees(self.search_target_heading))

                # end search if we've gone all the way around
                if self.search_total_angle >= math.radians(360):
                    # if we never saw a balloon then just complete (return control to user or mission)
                    if self.search_balloon_pos is None:
                        print "Search did not find balloon, giving up"
                        self.search_state = 0
                        self.complete()
                    else:
                        # update target heading towards closest balloon and send to flight controller
                        self.search_target_heading = self.search_balloon_heading
                        self.condition_yaw(math.degrees(self.search_target_heading))
                        self.search_state = 2   # advance towards rotating to best balloon stage
                        print "best balloon at %f" % math.degrees(self.search_balloon_heading)

        # search_state = 2: rotating to best balloon and double checking
        elif (self.search_state == 2):
            # check yaw is close to target
            if math.fabs(wrap_PI(self.vehicle.attitude.yaw - self.search_target_heading)) < math.radians(5):
                # reset targeting start time
                if self.targeting_start_time == 0:
                    self.targeting_start_time = time.time()

                # if targeting time has elapsed, double check the visible balloon is within acceptable limits
                if (time.time() - self.targeting_start_time > self.targeting_delay_time):
                    if self.balloon_found and (not self.balloon_pos is None) and (self.mission_alt_min == 0 or self.balloon_pos.z >= self.mission_alt_min) and (self.mission_alt_max == 0 or self.balloon_pos.z <= self.mission_alt_max) and (self.mission_distance_max == 0 or self.balloon_distance <= self.mission_distance_max):
                        # balloon is within limits so reset balloon target
                        self.search_balloon_pos = self.balloon_pos
                        self.search_balloon_heading = self.balloon_heading
                        self.search_balloon_pitch_top = self.balloon_pitch_top
                        self.search_balloon_distance = self.balloon_distance
                        # move to final heading
                        self.search_target_heading = self.search_balloon_heading
                        self.condition_yaw(math.degrees(self.search_target_heading))
                        # move to finalise yaw state
                        self.search_state = 3
                        print "Balloon was near expected heading, finalising yaw to %f" % (math.degrees(self.search_balloon_heading))

                    # we somehow haven't found the balloon when we've turned back to find it so giveup
                    elif (time.time() - self.targeting_start_time) > (self.targeting_delay_time + 1.0):
                        print "Balloon was not at expected heading: %f, giving up" % (math.degrees(self.search_target_heading))
                        self.search_state = 0
                        self.complete()

        # search_state = 3: finalising yaw
        elif (self.search_state == 3):
            # check yaw is close to target
            if math.fabs(wrap_PI(self.vehicle.attitude.yaw - self.search_target_heading)) < math.radians(5):
                # complete search, move should take-over
                # Note: we don't check again for the balloon, but surely it's still there
                self.search_state = 0
                print "Finalised yaw to %f, beginning run" % (math.degrees(self.search_balloon_heading))

    # move_to_balloon - velocity controller to drive vehicle to balloon
    #    analyze_image should have been called prior to this and filled in self.balloon_found, balloon_pitch, balloon_heading, balloon_distance 
    def move_to_balloon(self):

        # exit immediately if we are not controlling the vehicle
        if not self.controlling_vehicle:
            return

        # get current time
        now = time.time()

        # exit immediately if it's been too soon since the last update
        if (now - self.guided_last_update) < self.vel_update_rate:
            return;

        # if we have a new balloon position recalculate velocity vector
        if (self.balloon_found):

            # calculate change in yaw since we began the search
            yaw_error = wrap_PI(self.balloon_heading - self.search_balloon_heading)

            # calculate pitch vs ideal pitch angles.  This will cause to attempt to get to 5deg above balloon 
            pitch_error = wrap_PI(self.balloon_pitch_top - self.vel_pitch_target)

            # get time since last time velocity pid controller was run
            dt = self.vel_xy_pid.get_dt(2.0)

            # get speed towards balloon based on balloon distance
            speed = self.balloon_distance * self.vel_dist_ratio

            # apply min and max speed limit
            speed = min(speed, self.vel_speed_max)
            speed = max(speed, self.vel_speed_min)

            # apply acceleration limit
            speed_chg_max = self.vel_accel * dt
            speed = min(speed, self.vel_speed_last + speed_chg_max)
            speed = max(speed, self.vel_speed_last - speed_chg_max)

            # record speed for next iteration
            self.vel_speed_last = speed

            # calculate yaw correction and final yaw movement
            yaw_correction = self.vel_xy_pid.get_pid(yaw_error, dt)
            yaw_final = wrap_PI(self.search_balloon_heading + yaw_correction)

            # calculate pitch correction and final pitch movement
            pitch_correction = self.vel_z_pid.get_pid(pitch_error, dt)
            pitch_final = wrap_PI(self.search_balloon_pitch_top + pitch_correction)
            
            # calculate velocity vector we wish to move in
            self.guided_target_vel = balloon_finder.get_ef_velocity_vector(pitch_final, yaw_final, speed)

            # send velocity vector to flight controller
            self.send_nav_velocity(self.guided_target_vel[0], self.guided_target_vel[1], self.guided_target_vel[2])
            self.guided_last_update = now

        # if have not seen the balloon
        else:
            # if more than a few seconds has passed without seeing the balloon give up
            if now - self.last_spotted_time > self.lost_sight_timeout:
                if self.debug:
                    print "Lost Balloon, Giving up"
                # To-Do: start searching again or maybe slowdown?
                self.complete()

        # FIXME - check if vehicle altitude is too low
        # FIXME - check if we are too far from the desired flightplan

    def run(self):
        while not self.api.exit:

            # only process images once home has been initialised
            if self.check_home():

                # start video if required
                self.check_video_out()
    
                # check if we are controlling the vehicle
                self.check_status()

                # look for balloon in image
                self.analyze_image()

                # search or move towards balloon
                if self.search_state > 0:
                    # search for balloon
                    self.search_for_balloon()
                else:
                    # move towards balloon
                    self.move_to_balloon()

            # Don't suck up too much CPU, only process a new image occasionally
            time.sleep(0.05)

        if not self.use_simulator:
            balloon_video.stop_background_capture()

    # complete - balloon strategy has somehow completed so return control to the autopilot
    def complete(self):
        # debug
        if self.debug:
            print "Complete!"

        # stop the vehicle and give up control
        if self.controlling_vehicle:
            self.guided_target_vel = (0,0,0)
            self.send_nav_velocity(self.guided_target_vel[0], self.guided_target_vel[1], self.guided_target_vel[2])
            self.guided_last_update = time.time()

        # if in GUIDED mode switch back to LOITER
        if self.vehicle.mode.name == "GUIDED":
            self.vehicle.mode = VehicleMode("LOITER")
            self.vehicle.flush()

        # if in AUTO move to next command
        if self.vehicle.mode.name == "AUTO":
            self.advance_current_cmd();

        # flag we are not in control of the vehicle
        self.controlling_vehicle = False
            
        return