def new_settingsresponse_message(loaded_json: json, origin: int) -> bytes:
"""
takes in a request - executes search for settings and creates a response as bytes
:param loaded_json:
:param origin: is this a response of drone or ground station
:return: a complete response packet as bytes
"""
complete_response = {}
complete_response['destination'] = DBCommProt.DB_DST_GCS.value
complete_response['type'] = DBCommProt.DB_TYPE_SETTINGS_RESPONSE.value
complete_response['response'] = loaded_json['request']
complete_response['origin'] = origin
complete_response['id'] = loaded_json['id']
if loaded_json['request'] == DBCommProt.DB_REQUEST_TYPE_DB.value:
if 'settings' in loaded_json:
complete_response = read_dronebridge_settings(
complete_response, True, loaded_json) # can return None
else:
complete_response = read_dronebridge_settings(
complete_response, False, None) # can return None
elif loaded_json['request'] == DBCommProt.DB_REQUEST_TYPE_WBC.value:
db_log("DB_COMM_PROTO: ERROR - WBC settings read unsupported!",
ident=LOG_ERR)
return new_error_response_message("WBC settings read unsupported",
origin, loaded_json['id'])
if complete_response is None:
return new_error_response_message(
"Could not read DroneBridgeLib config", origin, loaded_json['id'])
response = json.dumps(complete_response)
crc32 = binascii.crc32(str.encode(response))
return response.encode() + crc32.to_bytes(
4, byteorder='little', signed=False)
def process_comm_proto(db_comm_message_bytes: bytes, _tcp_connections: list):
"""
Process and route communication messages on the ground station side
:param _tcp_connections: List of sockets of connected tcp clients
:param db_comm_message_bytes: Raw byte representation of the message
"""
try:
comm_json = parse_comm_message(db_comm_message_bytes)
if comm_json is not None:
if comm_json['destination'] == DBCommProt.DB_DST_GND.value:
message = process_db_comm_protocol(comm_json, DBDir.DB_TO_UAV)
if message != "":
sendto_tcp_clients(message, _tcp_connections)
elif comm_json['destination'] == DBCommProt.DB_DST_GND_UAV.value:
# Always process ping requests right away! Do not wait for UAV response!
if comm_json['type'] == DBCommProt.DB_TYPE_PING_REQUEST.value:
message = process_db_comm_protocol(comm_json,
DBDir.DB_TO_UAV)
sendto_tcp_clients(message, _tcp_connections)
db.sendto_uav(db_comm_message_bytes,
DBPort.DB_PORT_COMMUNICATION)
else:
db_log(
f"DB_COMM_GND: Destination 2 (GND & UAV) is only supported for ping messages",
ident=LOG_WARNING)
message = new_error_response_message(
'Destination 2 (GND & UAV) is unsupported for non ping msgs',
DBCommProt.DB_ORIGIN_GND.value, comm_json['id'])
sendto_tcp_clients(message, _tcp_connections)
elif comm_json['destination'] == DBCommProt.DB_DST_PER.value:
db.sendto_uav(db_comm_message_bytes,
DBPort.DB_PORT_COMMUNICATION)
elif comm_json['destination'] == DBCommProt.DB_DST_GCS.value:
sendto_tcp_clients(db_comm_message_bytes, _tcp_connections)
elif comm_json['destination'] == DBCommProt.DB_DST_UAV.value:
db_log("DB_COMM_GND: Forwarding msg to UAV", LOG_DEBUG)
db.sendto_uav(db_comm_message_bytes,
DBPort.DB_PORT_COMMUNICATION)
else:
db_log("DB_COMM_GND: Unknown message type", ident=LOG_ERR)
error_resp = new_error_response_message(
'DB_COMM_GND: Unknown message type',
DBCommProt.DB_ORIGIN_GND.value, comm_json['id'])
sendto_tcp_clients(error_resp, _tcp_connections)
else:
db_log("DB_COMM_GND: Corrupt message", ident=LOG_ERR)
error_resp = new_error_response_message(
'DB_COMM_GND: Corrupt message', DBCommProt.DB_ORIGIN_GND.value,
0)
sendto_tcp_clients(error_resp, _tcp_connections)
except (UnicodeDecodeError, ValueError):
db_log(
"DB_COMM_GND: Command could not be processed correctly! (UnicodeDecodeError, ValueError)",
ident=LOG_ERR)
def sendto_tcp_clients(data_bytes: bytes, _tcp_connections: list):
"""
Send to all connected TCP clients
:param data_bytes: Payload to send
:param _tcp_connections: List of socket objects to use for sending
"""
db_log("DB_COMM_GND: Responding ...")
for connected_socket in _tcp_connections:
if connected_socket.sendall(data_bytes) is not None:
db_log("DB_COMM_GND:\tShit!", ident=LOG_ERR)
def change_settings(loaded_json: json, origin: int) -> bytes:
"""takes a settings change request - executes it - returns a encoded settings change success message"""
worked = False
if loaded_json['change'] == DBCommProt.DB_REQUEST_TYPE_DB.value:
worked = change_settings_db(loaded_json)
elif loaded_json['change'] == DBCommProt.DB_REQUEST_TYPE_WBC.value:
db_log("DB_COMM_PROTO: Error - WBC settings change not supported",
ident=LOG_ERR)
worked = False
if worked:
return new_settingschangesuccess_message(origin, loaded_json['id'])
else:
return new_error_response_message('Could not change settings', origin,
loaded_json['id'])
def change_settings_db(loaded_json: json) -> bool:
try:
with open(PATH_DRONEBRIDGE_SETTINGS, 'r+') as file:
lines = file.readlines()
for section in loaded_json['settings']:
for key in loaded_json['settings'][section]:
for index, line in enumerate(lines):
if line.startswith(key + "="):
lines[index] = key + "=" + loaded_json['settings'][
section][key] + "\n"
file.seek(0, 0)
for line in lines:
file.write(line)
file.truncate()
file.flush()
os.fsync(file.fileno())
except Exception as ex:
db_log(f"DB_COMM_PROTO: Error writing DroneBridgeLib settings: {ex}",
ident=LOG_ERR)
return False
return True
def normalize_jscal_axis(device="/dev/input/js0"):
"""
Reads the raw min and max values that the RC-HID will send to the ground station and calculates the calibration
parameters to that the full range is used with no dead zone. The calibration is stored via "jscal-store".
.. note:: This function does not calibrate the joystick! The user needs to calibrate the RC itself. This function just
tells the system to not use any dead zone and makes sure the full range of the output is being used
:param device: The device descriptor
:return:
"""
devices = [evdev.InputDevice(path) for path in evdev.list_devices()]
dev_capabilitys_list = devices[0].capabilities().get(3)
if dev_capabilitys_list is not None:
num_joystick_axis = len(dev_capabilitys_list)
calibration_string = str(num_joystick_axis)
for i in range(num_joystick_axis):
absInfo = dev_capabilitys_list[i][1]
minimum = absInfo[
1] # minimum value the RC will send for the first axis - raw value!
maximum = absInfo[
2] # maximum value the RC will send for the first axis - raw value!
center_value = int((minimum + maximum) / 2)
correction_coeff_min = int(536854528 / (maximum - center_value))
correction_coeff_max = int(536854528 / (maximum - center_value))
calibration_string = calibration_string + ",1,0," + str(center_value) + "," + str(center_value) + "," \
+ str(correction_coeff_min) + "," + str(correction_coeff_max)
db_log("DB_COMM_PROTO: Calibrating:")
db_log(calibration_string)
call(["jscal", device, "-s", calibration_string])
db_log("DB_COMM_PROTO: Saving calibration")
call(["jscal-store", device])
def read_dronebridge_settings(response_header: dict, specific_request: bool,
requested_settings: json) -> json or None:
"""
Read settings from file and create a valid packet
:param response_header: Everything but the settings part of the message as a dict
:param specific_request: Is it a general or specific settings request: True|False
:param requested_settings: A request json
:return: The complete json with settings
"""
config = configparser.ConfigParser()
config.optionxform = str
response_settings = {} # settings object that gets sent
config.read(PATH_DRONEBRIDGE_SETTINGS)
if not config.read(PATH_DRONEBRIDGE_SETTINGS):
db_log("DB_COMM_PROTO: Error reading DroneBridgeLib config", LOG_ERR)
return None
if specific_request:
for section in requested_settings['settings']:
temp_dict = {}
for requested_setting in requested_settings['settings'][section]:
if requested_setting in config[section]:
temp_dict[requested_setting] = config.get(
section, requested_setting)
response_settings[section] = temp_dict
else:
for section in requested_settings['settings']:
temp_dict = {}
for requested_setting in requested_settings['settings'][section]:
if requested_setting in config[section]:
if requested_setting not in db_settings_blacklist:
temp_dict[requested_setting] = config.get(
section, requested_setting)
response_settings[section] = temp_dict
response_header['settings'] = response_settings
return response_header
def parse_comm_message(raw_data_encoded: bytes) -> None or json:
extracted_info = comm_message_extract_info(
raw_data_encoded) # returns json bytes [0] and crc bytes [1]
try:
loaded_json = json.loads(extracted_info[0].decode())
if not comm_crc_correct(extracted_info): # Check CRC
db_log("DB_COMM_PROTO: Communication message: invalid CRC",
ident=LOG_ERR)
return None
return loaded_json
except UnicodeDecodeError:
db_log("DB_COMM_PROTO: Invalid command: Could not decode json message",
ident=LOG_ERR)
return None
except ValueError:
db_log("DB_COMM_PROTO: ValueError on decoding json", ident=LOG_ERR)
return None
for connected_socket in _tcp_connections:
if connected_socket.sendall(data_bytes) is not None:
db_log("DB_COMM_GND:\tShit!", ident=LOG_ERR)
if __name__ == "__main__":
signal.signal(signal.SIGINT, signal_handler)
signal.signal(signal.SIGTERM, signal_handler)
parsedArgs = parse_arguments()
mode = parsedArgs.mode
list_interfaces = parsedArgs.DB_INTERFACES
comm_id = bytes([parsedArgs.comm_id])
comp_mode = parsedArgs.comp_mode
frame_type = int(parsedArgs.frametype)
db_log("DB_COMM_GND: Communication ID: " +
str(int.from_bytes(comm_id, byteorder='little')) + " (" +
str(comm_id.hex()) + ")")
db = DroneBridge(DBDir.DB_TO_UAV,
list_interfaces,
DBMode.MONITOR,
comm_id,
DBPort.DB_PORT_COMMUNICATION,
tag="DB_COMM_GND",
db_blocking_socket=False,
frame_type=frame_type,
compatibility_mode=comp_mode)
# We use a stupid tcp implementation where all connected clients receive the data sent by the UAV. GCS must
# identify the relevant messages based on the id in every communication message. Robust & multiple clients possible
tcp_master = open_tcp_socket()
tcp_connections = []
TCP_BUFFER_SIZE = 2048
def process_db_comm_protocol(loaded_json: json,
comm_direction: DBDir) -> bytes:
"""
Execute the command given in the DroneBridgeLib communication packet
:param loaded_json: The message to process
:param comm_direction: The direction of the local program instance in which it is sending
:return: correct response message
"""
message = ""
if loaded_json['type'] == DBCommProt.DB_TYPE_SETTINGS_REQUEST.value:
if comm_direction == DBDir.DB_TO_UAV:
message = new_settingsresponse_message(
loaded_json, DBCommProt.DB_ORIGIN_GND.value)
else:
message = new_settingsresponse_message(
loaded_json, DBCommProt.DB_ORIGIN_UAV.value)
elif loaded_json['type'] == DBCommProt.DB_TYPE_SETTINGS_CHANGE.value:
if comm_direction == DBDir.DB_TO_UAV:
message = change_settings(loaded_json,
DBCommProt.DB_ORIGIN_GND.value)
else:
message = change_settings(loaded_json,
DBCommProt.DB_ORIGIN_UAV.value)
elif loaded_json['type'] == DBCommProt.DB_TYPE_SYS_IDENT_REQUEST.value:
if comm_direction == DBDir.DB_TO_UAV:
message = create_sys_ident_response(loaded_json['id'],
DBCommProt.DB_ORIGIN_GND.value)
else:
message = create_sys_ident_response(loaded_json['id'],
DBCommProt.DB_ORIGIN_UAV.value)
elif loaded_json['type'] == DBCommProt.DB_TYPE_PING_REQUEST.value:
if comm_direction == DBDir.DB_TO_UAV:
message = new_ping_response_message(loaded_json['id'],
DBCommProt.DB_ORIGIN_GND.value)
else:
message = new_ping_response_message(loaded_json['id'],
DBCommProt.DB_ORIGIN_UAV.value)
elif loaded_json['type'] == DBCommProt.DB_TYPE_CAMSELECT.value:
change_cam_selection(loaded_json['cam'])
message = new_ack_message(DBCommProt.DB_ORIGIN_UAV.value,
loaded_json['id'])
elif loaded_json['type'] == DBCommProt.DB_TYPE_ADJUSTRC.value:
normalize_jscal_axis(loaded_json['device'])
message = new_ack_message(DBCommProt.DB_ORIGIN_GND.value,
loaded_json['id'])
elif loaded_json['type'] == DBCommProt.DB_TYPE_PARAM_REQ.value:
if comm_direction == DBDir.DB_TO_UAV:
message = new_settings_param_response(
loaded_json['id'], DBCommProt.DB_ORIGIN_GND.value)
else:
message = new_settings_param_response(
loaded_json['id'], DBCommProt.DB_ORIGIN_UAV.value)
elif loaded_json['type'] == DBCommProt.DB_TYPE_SECTION_REQ.value:
if comm_direction == DBDir.DB_TO_UAV:
message = new_settings_section_response(
loaded_json['id'], DBCommProt.DB_ORIGIN_GND.value)
else:
message = new_settings_section_response(
loaded_json['id'], DBCommProt.DB_ORIGIN_UAV.value)
else:
if comm_direction == DBDir.DB_TO_UAV:
message = new_error_response_message(
'unsupported message type', DBCommProt.DB_ORIGIN_GND.value,
loaded_json['id'])
else:
message = new_error_response_message(
'unsupported message type', DBCommProt.DB_ORIGIN_UAV.value,
loaded_json['id'])
db_log("DB_COMM_PROTO: Unknown message type", ident=LOG_ERR)
return message
def signal_handler(signal, frame):
global keep_running
keep_running = False
if __name__ == "__main__":
signal.signal(signal.SIGINT, signal_handler)
signal.signal(signal.SIGTERM, signal_handler)
parsedArgs = parse_arguments()
mode = parsedArgs.mode
list_interfaces = parsedArgs.DB_INTERFACES
comm_id = bytes([parsedArgs.comm_id])
comp_mode = parsedArgs.comp_mode
frame_type = int(parsedArgs.frametype)
db_log("DB_COMM_AIR: Communication ID: " +
str(int.from_bytes(comm_id, byteorder='little')) + " (" +
str(comm_id.hex()) + ")")
db = DroneBridge(DBDir.DB_TO_GND,
list_interfaces,
DBMode.MONITOR,
comm_id,
DBPort.DB_PORT_COMMUNICATION,
tag="DB_COMM_AIR",
db_blocking_socket=True,
frame_type=frame_type,
compatibility_mode=comp_mode)
first_run = True
while keep_running:
if first_run:
db.clear_socket_buffers()
first_run = False