def _play_animation(**kwargs):
start_time = float(kwargs["time"]) # TODO
if (animation.get_id() == 'No animation'):
print("Can't start animation without animation file!")
return
print("Start time = {}, wait for {} seconds".format(start_time, time.time()-start_time))
frames = animation.load_animation(os.path.abspath("animation.csv"),
x0=client.active_client.X0 + client.active_client.X0_COMMON,
y0=client.active_client.Y0 + client.active_client.Y0_COMMON,
)
task_manager.add_task(start_time, 0, animation.takeoff,
task_kwargs={
"z": client.active_client.TAKEOFF_HEIGHT,
"timeout": client.active_client.TAKEOFF_TIME,
"safe_takeoff": client.active_client.SAFE_TAKEOFF,
#"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
}
)
rfp_time = start_time + client.active_client.TAKEOFF_TIME
task_manager.add_task(rfp_time, 0, animation.execute_frame,
task_kwargs={
"point": animation.convert_frame(frames[0])[0],
"color": animation.convert_frame(frames[0])[1],
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
"flight_func": FlightLib.reach_point,
}
)
frame_time = rfp_time + client.active_client.RFP_TIME
frame_delay = 0.125 # TODO from animation file
for frame in frames:
task_manager.add_task(frame_time, 0, animation.execute_frame,
task_kwargs={
"point": animation.convert_frame(frame)[0],
"color": animation.convert_frame(frame)[1],
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
"flight_func": FlightLib.navto,
}
)
frame_time += frame_delay
land_time = frame_time + client.active_client.LAND_TIME
task_manager.add_task(land_time, 0, animation.land,
task_kwargs={
"timeout": client.active_client.TAKEOFF_TIME,
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
},
)
def update_telemetry_slow(self):
self.animation_id = animation.get_id()
self.git_version = self.get_git_version()
try:
self.calibration_status = get_calibration_status()
self.system_status = get_sys_status()
self.battery = self.get_battery(self.ros_telemetry)
except rospy.ServiceException:
rospy.logdebug("Some service is unavailable")
self.selfcheck = ["WAIT_ROS"]
except AttributeError as e:
rospy.logdebug(e)
except rospy.TransportException as e:
rospy.logdebug(e)
def _response_animation_id(*args, **kwargs):
# Load animation
result = animation.get_id()
if result != 'No animation':
logger.debug("Saving corrected animation")
frames = animation.load_animation(
os.path.abspath("animation.csv"),
x0=client.active_client.X0 + client.active_client.X0_COMMON,
y0=client.active_client.Y0 + client.active_client.Y0_COMMON,
z0=client.active_client.Z0 + client.active_client.Z0_COMMON,
x_ratio=client.active_client.X_RATIO,
y_ratio=client.active_client.Y_RATIO,
z_ratio=client.active_client.Z_RATIO,
)
# Correct start and land frames in animation
corrected_frames, start_action, start_delay = animation.correct_animation(
frames,
check_takeoff=client.active_client.TAKEOFF_CHECK,
check_land=client.active_client.LAND_CHECK,
)
logger.debug("Start action: {}".format(start_action))
# Save corrected animation
animation.save_corrected_animation(corrected_frames)
return result
def telemetry_loop():
global telemetry, emergency
last_state = []
equal_state_counter = 0
max_count = 2
tasks_cleared = False
rate = rospy.Rate(client.active_client.TELEM_FREQ)
while not rospy.is_shutdown():
telemetry = telemetry._replace(animation_id=animation.get_id())
telemetry = telemetry._replace(git_version=subprocess.check_output(
"git log --pretty=format:'%h' -n 1", shell=True))
x_start, y_start = animation.get_start_xy(
os.path.abspath("animation.csv"),
x_ratio=client.active_client.X_RATIO,
y_ratio=client.active_client.Y_RATIO,
)
x_delta = client.active_client.X0 + client.active_client.X0_COMMON
y_delta = client.active_client.Y0 + client.active_client.Y0_COMMON
z_delta = client.active_client.Z0 + client.active_client.Z0_COMMON
if not math.isnan(x_start):
telemetry = telemetry._replace(
start_position='{:.2f} {:.2f} {:.2f}'.format(
x_start + x_delta, y_start + y_delta, z_delta))
else:
telemetry = telemetry._replace(start_position='NO_POS')
services_unavailable = FlightLib.check_ros_services_unavailable()
if not services_unavailable:
try:
ros_telemetry = FlightLib.get_telemetry_locked(
client.active_client.FRAME_ID)
if ros_telemetry.connected:
telemetry = telemetry._replace(armed=ros_telemetry.armed)
telemetry = telemetry._replace(
battery_v='{:.2f}'.format(ros_telemetry.voltage))
batt_empty_param = get_param('BAT_V_EMPTY')
batt_charged_param = get_param('BAT_V_CHARGED')
batt_cells_param = get_param('BAT_N_CELLS')
if batt_empty_param.success and batt_charged_param.success and batt_cells_param.success:
batt_empty = batt_empty_param.value.real
batt_charged = batt_charged_param.value.real
batt_cells = batt_cells_param.value.integer
try:
telemetry = telemetry._replace(
battery_p='{}'.format(
int(
min((ros_telemetry.voltage /
batt_cells - batt_empty) /
(batt_charged - batt_empty) *
100., 100))))
except ValueError:
telemetry = telemetry._replace(battery_p='nan')
else:
telemetry = telemetry._replace(battery_p='nan')
telemetry = telemetry._replace(
calibration_status=get_calibration_status())
telemetry = telemetry._replace(
system_status=get_sys_status())
telemetry = telemetry._replace(mode=ros_telemetry.mode)
check = FlightLib.selfcheck()
if not check:
check = "OK"
telemetry = telemetry._replace(selfcheck=str(check))
if not math.isnan(ros_telemetry.x):
telemetry = telemetry._replace(
current_position='{:.2f} {:.2f} {:.2f} {:.1f} {}'.
format(ros_telemetry.x, ros_telemetry.y,
ros_telemetry.z,
math.degrees(ros_telemetry.yaw),
client.active_client.FRAME_ID))
else:
telemetry = telemetry._replace(
current_position='NO_POS in {}'.format(
client.active_client.FRAME_ID))
else:
telemetry = telemetry._replace(battery_v='nan')
telemetry = telemetry._replace(battery_p='nan')
telemetry = telemetry._replace(calibration_status='NO_FCU')
telemetry = telemetry._replace(system_status='NO_FCU')
telemetry = telemetry._replace(mode='NO_FCU')
telemetry = telemetry._replace(selfcheck='NO_FCU')
telemetry = telemetry._replace(current_position='NO_POS')
except rospy.ServiceException:
logger.debug("Some service is unavailable")
except AttributeError as e:
logger.debug(e)
except rospy.TransportException as e:
logger.debug(e)
else:
telemetry = telemetry._replace(selfcheck='WAIT_ROS')
if client.active_client.TELEM_TRANSMIT:
try:
client.active_client.server_connection.send_message(
'telem',
args={'message': create_telemetry_message(telemetry)})
except AttributeError as e:
logger.debug(e)
if client.active_client.CLEAR_TASKS_WHEN_EMERGENCY:
mode = telemetry.mode
armed = telemetry.armed
last_task = task_manager.get_last_task_name()
state = [mode, armed, last_task]
if state == last_state:
equal_state_counter += 1
else:
equal_state_counter = 0
external_interruption = (mode != "OFFBOARD" and armed == True
and last_task not in [None, 'land'])
log_msg = ''
if emergency and external_interruption:
log_msg = "emergency and external interruption"
elif emergency:
log_msg = "emergency"
elif external_interruption:
log_msg = "external interruption"
logger.info(
"Possible expernal interruption, state_counter = {}".
format(equal_state_counter))
if emergency or (external_interruption
and equal_state_counter >= max_count):
if not tasks_cleared:
logger.info(
"Clear task manager because of {}".format(log_msg))
logger.info("Mode: {} | armed: {} | last task: {} ".format(
mode, armed, last_task))
task_manager.reset()
FlightLib.reset_delta()
tasks_cleared = True
equal_state_counter = 0
else:
tasks_cleared = False
last_state = state
if client.active_client.LOG_CPU_AND_MEMORY:
cpu_usage = psutil.cpu_percent(interval=None, percpu=True)
mem_usage = psutil.virtual_memory().percent
cpu_temp_info = psutil.sensors_temperatures()['cpu-thermal'][0]
cpu_temp = cpu_temp_info.current
# https://github.com/raspberrypi/documentation/blob/JamesH65-patch-vcgencmd-vcdbg-docs/raspbian/applications/vcgencmd.md
throttled_hex = subprocess.check_output("vcgencmd get_throttled",
shell=True).split('=')[1]
under_voltage = bool(int(bin(int(throttled_hex, 16))[2:][-1]))
power_state = 'normal' if not under_voltage else 'under voltage!'
if cpu_temp_info.critical:
cpu_temp_state = 'critical'
elif cpu_temp_info.high:
cpu_temp_state = 'high'
else:
cpu_temp_state = 'normal'
logger.info(
"CPU usage: {} | Memory: {} % | T: {} ({}) | Power: {}".format(
cpu_usage, mem_usage, cpu_temp, cpu_temp_state,
power_state))
delta = FlightLib.get_delta()
logger.info("Delta: {}".format(delta))
if delta > client.active_client.LAND_POS_DELTA:
_command_land()
rate.sleep()
def _play_animation(*args, **kwargs):
start_time = float(kwargs["time"])
# Check if animation file is available
if animation.get_id() == 'No animation':
logger.error("Can't start animation without animation file!")
return
task_manager.reset(interrupt_next_task=False)
logger.info("Start time = {}, wait for {} seconds".format(
start_time, start_time - time.time()))
# Load animation
frames = animation.load_animation(
os.path.abspath("animation.csv"),
x0=client.active_client.X0 + client.active_client.X0_COMMON,
y0=client.active_client.Y0 + client.active_client.Y0_COMMON,
z0=client.active_client.Z0 + client.active_client.Z0_COMMON,
x_ratio=client.active_client.X_RATIO,
y_ratio=client.active_client.Y_RATIO,
z_ratio=client.active_client.Z_RATIO,
)
# Correct start and land frames in animation
corrected_frames, start_action, start_delay = animation.correct_animation(
frames,
check_takeoff=client.active_client.TAKEOFF_CHECK,
check_land=client.active_client.LAND_CHECK,
)
# Choose start action
if start_action == 'takeoff':
# Takeoff first
task_manager.add_task(
start_time,
0,
animation.takeoff,
task_kwargs={
"z": client.active_client.TAKEOFF_HEIGHT,
"timeout": client.active_client.TAKEOFF_TIME,
"safe_takeoff": client.active_client.SAFE_TAKEOFF,
# "frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
})
# Fly to first point
rfp_time = start_time + client.active_client.TAKEOFF_TIME
task_manager.add_task(
rfp_time,
0,
animation.execute_frame,
task_kwargs={
"point": animation.convert_frame(corrected_frames[0])[0],
"color": animation.convert_frame(corrected_frames[0])[1],
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
"flight_func": FlightLib.reach_point,
})
# Calculate first frame start time
frame_time = rfp_time + client.active_client.RFP_TIME
elif start_action == 'arm':
# Calculate start time
start_time += start_delay
# Arm
#task_manager.add_task(start_time, 0, FlightLib.arming_wrapper,
# task_kwargs={
# "state": True
# }
# )
frame_time = start_time # + 1.0
point, color, yaw = animation.convert_frame(corrected_frames[0])
task_manager.add_task(frame_time,
0,
animation.execute_frame,
task_kwargs={
"point": point,
"color": color,
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
"flight_func": FlightLib.navto,
"auto_arm": True,
})
# Calculate first frame start time
frame_time += client.active_client.FRAME_DELAY # TODO Think about arming time
logger.debug(task_manager.task_queue)
# Play animation file
for frame in corrected_frames:
point, color, yaw = animation.convert_frame(frame)
if client.active_client.YAW == "animation":
yaw = frame["yaw"]
else:
yaw = math.radians(float(client.active_client.YAW))
task_manager.add_task(frame_time,
0,
animation.execute_frame,
task_kwargs={
"point": point,
"color": color,
"yaw": yaw,
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
"flight_func": FlightLib.navto,
})
frame_time += frame["delay"]
# Calculate land_time
land_time = frame_time + client.active_client.LAND_TIME
# Land
task_manager.add_task(
land_time,
0,
animation.land,
task_kwargs={
"timeout": client.active_client.LAND_TIMEOUT,
"frame_id": client.active_client.FRAME_ID,
"use_leds": client.active_client.USE_LEDS,
},
)
def _response_animation_id():
return animation.get_id()
