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
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
