def load(firmware_path):
logger.info("loading %s", firmware_path)
dev_name = create_usb_serial()
if dev_name is None:
return False
# load firmware
start_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
ret = subprocess.call(
["px_uploader.py",
"--port=%s" % dev_pattern_usb, firmware_path])
if ret != 0:
loaded = False
logger.error("loading firmware")
else:
loaded = True
end_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
logger.info("firmware loaded in %0.3f sec",
(end_us - start_us) / 1000000.0)
if loaded:
# firmware loaded; wait for heartbeat on telemetry port
# This allows for the baud rate to change along with the firmware load.
start_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
# 0.6.x ... 1.0.5 has a pixhawk build at 115200 baud
# internal versions might have a pixhawk build at 921600 baud
# 1.1.5 and later have a pixhawk build at 115200 baud
baudlist = get_baudlist(None, 115200)
hb_quit = False
hb_time_us = None
wait_us = None
hb = None
while not hb_quit:
for baud in baudlist:
logger.debug("trying %d", baud)
m = create_tty_mavlink(baud)
if m is None:
logger.error("creating tty mavlink connection")
hb_quit = True
break # for baud
# flush input - could we have data from before flashing?
flush_bytes = m.port.inWaiting()
if flush_bytes > 0:
logger.info("flushing %d bytes", flush_bytes)
m.port.flushInput()
logger.debug("waiting for heartbeat")
hb = m.recv_match(type='HEARTBEAT', blocking=True, timeout=1.1)
wait_us = clock.gettime_us(clock.CLOCK_MONOTONIC) - start_us
if hb is None:
logger.debug("timeout waiting for first heartbeat")
else:
# insisting on a second heartbeat was in response to
# occasionally detecting a heartbeat at the wrong baud
# when the baud changes with the firmware (!)
logger.debug("got first heartbeat")
hb_time_us = clock.gettime_us(
clock.CLOCK_MONOTONIC) - start_us
hb = m.recv_match(type='HEARTBEAT',
blocking=True,
timeout=1.1)
if hb is None:
# this has been observed to happen (rarely)
logger.info(
"timeout waiting for second heartbeat at %d after loading",
baud)
# ...and continue with the next baud rate
else:
logger.debug("got second heartbeat")
m.close()
if hb is not None:
set_baud(baud)
hb_quit = True
break # for baud
# 0.1.0 comes up in about 25 sec
# 1.0.8 comes up in about 8 sec
# 1.1.4 comes up in about 15 sec
if wait_us > 45000000: # 45 sec
hb_quit = True # to exit the while loop
break # exit the baud loop
### for baud in baudlist
### while !hb_quit
if hb is None:
logger.error("timeout waiting for heartbeat after loading")
else:
logger.info("heartbeat after loading in %0.3f sec",
hb_time_us / 1000000.0)
delete_usb_serial()
return loaded
def get_version():
config = ConfigParser.SafeConfigParser()
config.optionxform = str
config.read(sololink_conf)
serial_dev = config_get(config, config_dev_name)
if serial_dev is None:
logger.error("reading %s from %s", config_dev_name, sololink_conf)
return None
logger.debug("%s = %s", config_dev_name, serial_dev)
serial_flow = config_getbool(config, config_flow_name, True)
logger.debug("%s = %s", config_flow_name, serial_flow)
serial_baud = config_getint(config, config_baud_name)
if serial_baud is None:
logger.error("reading %s from %s", config_baud_name, sololink_conf)
return None
logger.debug("%s = %d", config_baud_name, serial_baud)
m = mavutil.mavlink_connection(serial_dev, baud=serial_baud)
m.set_rtscts(serial_flow)
version = {}
# Read version. Use AUTOPILOT_VERSION message to get the hashes, and use
# the STATUSTEXT returned at the start a parameter dump to get the x.y.z
# version.
m.mav.autopilot_version_request_send(m.target_system, m.target_component)
start_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
av = m.recv_match(type='AUTOPILOT_VERSION', blocking=True, timeout=2)
end_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
if av is not None:
version["ardupilot_git_hash"] = "".join(
chr(e) for e in av.flight_custom_version)
version["px4_git_hash"] = "".join(
chr(e) for e in av.middleware_custom_version)
version["nuttx_git_hash"] = "".join(
chr(e) for e in av.os_custom_version)
logger.debug("git hashes received in %0.3f sec",
(end_us - start_us) / 1000000.0)
else:
logger.warning("no version hashes received")
m.mav.param_request_list_send(m.target_system, m.target_component)
start_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
end_us = start_us
timeout_us = 5 * 1000000
# Loop because we might get a STATUSTEXT without the version first.
while (end_us - start_us) < timeout_us:
st = m.recv_match(type='STATUSTEXT', blocking=True, timeout=5)
end_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
if st is not None:
logger.info("Status Text: %s" % st)
# "APM:Copter solo-0.1.2 (b2dacc52)"
match = re.match("APM:.*?solo-([0-9]+\.[0-9]+\.[0-9]+)", st.text)
if match:
logger.debug("build version received in %0.3f sec",
(end_us - start_us) / 1000000.0)
version["build_version"] = match.group(1)
logger.info("build version %s", version_string(version))
m.close()
return version
# "ArduCopter V3.2.1 (b2dacc52)"
# This is matched in case someone is messing with their firmware
# Anything looking like a version x.y.z is pulled out
match = re.match(".*?([0-9]+\.[0-9]+\.[0-9]+)", st.text)
if match:
logger.warning("firmware is not specifically for solo")
logger.info("build version received in %0.3f sec",
(end_us - start_us) / 1000000.0)
version["build_version"] = match.group(1)
logger.info("build version %s", version_string(version))
m.close()
return version
### while end_us...
m.close()
logger.warning("no build version received")
return version
def checkPixhawkVersion():
global cube_version
# Setup serial comm to the pixhawk
config = ConfigParser.SafeConfigParser()
config.optionxform = str
config.read(sololink_conf)
serial_dev = config_get(config, config_dev_name)
if serial_dev is None:
logger.error("reading %s from %s", config_dev_name, sololink_conf)
return False
logger.debug("%s = %s", config_dev_name, serial_dev)
serial_flow = config_getbool(config, config_flow_name, True)
logger.debug("%s = %s", config_flow_name, serial_flow)
serial_baud = config_getint(config, config_baud_name)
if serial_baud is None:
logger.error("reading %s from %s", config_baud_name, sololink_conf)
return False
logger.debug("%s = %d", config_baud_name, serial_baud)
m = mavutil.mavlink_connection(serial_dev, baud=serial_baud)
m.set_rtscts(serial_flow)
timeout_us = 30 * 1000000
# First we'll check parameter INS_ACC_ID. On stock 3DR firmware, this param
# will not be found. With ArduCopter 3.5+, the parameter will be detected.
# A value of 1442082 is a green cube. A value of 1245474 is a stock cube.
logger.info("Checking parameter INS_ACC_ID to determine cube version")
start_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
end_us = start_us
while ((end_us - start_us) < timeout_us) and cube_version == 'unknown':
m.mav.param_request_read_send(m.target_system, m.target_component,
'INS_ACC_ID', -1)
param_check = m.recv_match(type='PARAM_VALUE',
blocking=True,
timeout=5)
end_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
if param_check is not None:
if param_check.get_type() == 'PARAM_VALUE' and str(
param_check.param_id) == 'INS_ACC_ID':
if str(param_check.param_value) == '1442082.0':
logger.info("Pixhawk 2.1 on board!")
cube_version = 'cube_21'
m.close()
return True
elif str(param_check.param_value) == '1245474.0':
logger.info(
"Pixhawk 2.0 on board. But not stock solo firmware")
cube_version = 'cube_20'
m.close()
return True
logger.info("cube_version = %s", cube_version)
# If we've gotten to this point, it must have not found the INS_ACC_ID. So
# we'll check the compass #3 device ID instead. On a stock cube with stock
# stock FW, it will be 66286. On a stock with AC3.5, it is for some reason
# not found. So this only works for stock FW on a stock cube, which should
# be fine for all but a few people running master on a stock cube.
logger.info("Checking parameter COMPASS_DEV_ID3 to determine cube version")
start_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
end_us = start_us
while ((end_us - start_us) < timeout_us) and cube_version == 'unknown':
m.mav.param_request_read_send(m.target_system, m.target_component,
'COMPASS_DEV_ID3', -1)
param_check = m.recv_match(type='PARAM_VALUE',
blocking=True,
timeout=5)
end_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
if param_check is not None:
if param_check.get_type() == 'PARAM_VALUE' and str(
param_check.param_id) == 'COMPASS_DEV_ID3':
if str(param_check.param_value) == '263178.0':
logger.info("Pixhawk 2.1 on board!")
cube_version = 'cube_21'
m.close()
return True
elif str(param_check.param_value) == '66826.0':
logger.info("Pixhawk 2.0 on board. You should go green :)")
cube_version = 'cube_20'
m.close()
return True
logger.info("cube_version = %s", cube_version)
# Handle scenario where the external compass is unplugged, so what was
# compass #3 has become #2.
logger.info("Checking parameter COMPASS_DEV_ID2 to determine cube version")
start_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
end_us = start_us
while ((end_us - start_us) < timeout_us) and cube_version == 'unknown':
m.mav.param_request_read_send(m.target_system, m.target_component,
'COMPASS_DEV_ID2', -1)
param_check = m.recv_match(type='PARAM_VALUE',
blocking=True,
timeout=5)
end_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
if param_check is not None:
if param_check.get_type() == 'PARAM_VALUE' and str(
param_check.param_id) == 'COMPASS_DEV_ID2':
if str(param_check.param_value) == '263178.0':
logger.info("Pixhawk 2.1 on board!")
cube_version = 'cube_21'
m.close()
return True
elif str(param_check.param_value) == '66826.0':
logger.info("Pixhawk 2.0 on board. You should go green :)")
cube_version = 'cube_20'
m.close()
return True
logger.info("cube_version = %s", cube_version)
# If we've reached this point, we were unable to positively identify which
# cube is in the solo. Probably because something is malfunctioning, or
# because someone is running master on a stock cube. In either case, there
# is nothing more we can do, so returning false and leave cube_version unknown.
cube_version = 'unknown'
logger.info("Unable to determine cube version")
return False
"txBytes" : "txb", "txPackets" : "txp",
"txBitrate" : "txr", "signal" : "sig"
}
#RC lockout
rclockout = True
#if there is an unlock file or no lock file, we're unlocked
if os.path.isfile("/tmp/.rc_unlock"):
rclockout = False
elif (not os.path.isfile("/etc/.rc_lock")) and (not os.path.isfile("/mnt/rootfs.ro/etc/.rc_lock")):
rclockout = False
# how often to send a message
interval_us = 1000000
next_us = clock.gettime_us(clock.CLOCK_MONOTONIC) + interval_us
while True:
now_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
if next_us > now_us:
time.sleep((next_us - now_us) / 1000000.0)
next_us += interval_us
info = iw.link(if_name)
s = ""
for key in info:
if key in skip_list:
continue
if key in abbrev_list:
lbl = abbrev_list[key]
def run_connected():
global version_mismatch_log_time_us
ack_received = None
logger.info("establishing connection...")
controller_adrs = (controller_ip, controller_link_port)
confirmed = False
pending = False
pair_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
pair_sock.bind(("", 0)) # any port
pair_sock.settimeout(connect_ack_timeout)
send_error_logged = False
recv_error_logged = False
while True:
need_sleep = False
if rc_lock.locked():
locked = 1
else:
locked = 0
logger.debug("locked=%d", locked)
conn_req = struct.pack("<BBBB32s32s", pair.CMD_CONN_REQ, pair.SYS_SOLO,
0, locked, solo_sololink_version,
solo_firmware_version)
try:
pair_sock.sendto(conn_req, controller_adrs)
except Exception as e:
if not send_error_logged:
logger.info("error returned from sendto (%s)" % (e))
send_error_logged = True
need_sleep = True
pkt = None
else:
# sendto success; reset so we log the next error
send_error_logged = False
# If we got an error on send, we'll likely get an error from this
# recvfrom, leading to the correct processing (same as if we skipped
# this recvfrom).
try:
pkt, adrs = pair_sock.recvfrom(256)
except socket.timeout:
need_sleep = False
pkt = None
except Exception as e:
if not recv_error_logged:
logger.info("error returned from recvfrom (%s)" % (e))
recv_error_logged = True
need_sleep = True
pkt = None
else:
# recvfrom success; reset so we log the next error
recv_error_logged = False
now_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
if pkt is None:
if ack_received is not None and ack_received:
# first non-reply after ack received
logger.info("timeout waiting for ack")
ack_received = False
if not confirmed and wait_button(0):
logger.info("pairing button detected")
network_down()
return
if need_sleep:
# Could be here because of timeout waiting for packet, or
# socket error (e.g. out of range). If not a timeout, sleep
# a bit so as not to soak the CPU sending requests.
time.sleep(connect_request_interval)
# back to top to send next request
continue
# got a reply
if len(pkt) == pair.CONN_MSG_LEN and \
ord(pkt[0]) == pair.CMD_CONN_ACK and \
ord(pkt[1]) == pair.SYS_CONTROLLER:
if ord(pkt[2]) == pair.YES:
set_controller_versions(pkt)
if not confirmed:
confirmed = True
elif ack_received is not None and not ack_received:
# previous one timed out
logger.info("ack received after timeout")
ack_received = True
# Do this even if connection was already confirmed. It is
# possible that the other side started out on the wrong
# version, the connection was confirmed, the other side was
# updated, and now the versions match so we should unlock.
if (not check_versions) or (solo_sololink_version
== controller_sololink_version):
rc_lock.unlock_version()
else:
rc_lock.lock_version()
# logging is rate-limited here
if now_us > version_mismatch_log_time_us:
logger.info(
"version mismatch: solo=\"%s\", controller=\"%s\"",
solo_sololink_version, controller_sololink_version)
version_mismatch_log_time_us = now_us + version_mismatch_log_interval_us
# Change runlevel even if locked or versions incompatible.
# Apps look better if there is telemetry flowing and shotmgr
# is running
go_ready()
elif ord(pkt[2]) == pair.PEND:
if not pending:
logger.info("connection pending")
pending = True
else: # pair.NO
if not confirmed:
# Controller says no. This Solo knows the wifi password
# from a previous pairing, but the controller has since
# been re-paired to a different Solo.
logger.info("connection rejected")
network_down()
return
else:
# Controller said yes to a previous connect request, but
# is now saying no. We are already in runlevel 4;
# something is really messed up. Ignore the nack.
logger.error("connection was up, now rejected")
else:
# mystery packet!
logger.error("mystery response received: %s",
str([ord(c) for c in pkt]))
time.sleep(connect_request_interval)
gcs_sock.bind(("", 0))
read_socks = [solo_sock, gcs_sock]
# index by tuple (src_system, src_component)
# corrupt packets in (-1, -1)
packets_down = {}
packets_down_drops = {}
packets_down_total = 0
packets_up_total = 0
# list of tuples ("mac", "ip")
current_stations = []
now_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
# how often to update list of stations to send telemetry to
station_update_interval_us = long(5 * 1000000)
station_update_time_us = now_us
# how often to log packet counts
report_interval_us = long(10 * 1000000)
report_time_us = now_us + report_interval_us
got_gps_time = False
# last_down_sequence is indexed by a tuple (src_system, src_component)
# Each component has an independently running sequence, so we must keep track
# of each.
last_down_sequence = {}
# if the config file is not found, and empty list is returned and the
# "get" operations later fail
config.read(solo_conf)
# read configuration items
try:
app_address_file = config.get("solo", "appAddressFile")
except:
logger.error("error reading config from %s", solo_conf)
sys.exit(1)
packet_count = 0
byte_count = 0
now_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
log_interval_s = 10
log_interval_us = long(log_interval_s * 1000000)
log_time_us = now_us + log_interval_us
app_time_us = now_us
app_interval_us = 1000000
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.setsockopt(socket.IPPROTO_IP, socket.IP_TOS, 191) #Highest level VI
sock.bind((src_ip, src_port))
while True:
# forward a packet
pkt = sock.recv(4096)
def initialize():
start_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
led.blink(1000, 100)
print "pixhawk..."
baud = get_baud()
if baud == None:
print "pixhawk: ERROR checking baud"
logger.error("finding baud")
logger.error("pixhawk status: no response")
# pixhawk might be stuck in bootloader
# try to load if we have firmware, whether we found the baud or not
(firmware_path, firmware_version) = find_firmware()
if firmware_path is None:
logger.info("no new firmware file")
# Another backup would be to look in /firmware/loaded and load
# whatever's there if we did not get a heartbeat. Getting stuck in
# the bootloader has so far only been observed to happen when a
# programming is interrupted, in which case the firmware we were
# loading is still in /firmware.
elif os.path.exists("/log/.factory") and \
(baud is not None) and \
verify_usb():
logger.info("pixhawk: factory - not loading firmware")
move_firmware(firmware_path)
else:
print "pixhawk: loading firmware"
logger.info("%s:", firmware_path)
for v in firmware_version:
logger.info("%-20s %s", v, firmware_version[v])
if load(firmware_path):
move_firmware(firmware_path)
else:
print "pixhawk: ERROR loading firmware"
logger.error("pixhawk status: can't load")
# If .factory exists, we either found a baud, passed USB, and skipped
# loading pixhawk, or either did not find a baud or USB heartbeat, and
# loaded pixhawk.
os.system("rm -f /log/.factory")
# We have followed some combination of these to get here:
# * found baud | did not find baud
# * no firmware | firmware and flashed it | firmware and error flashing
#
# The cases that are known to happen:
# Normal cases
# * Found baud, there was no new firmware
# * Found baud, there was new firmware, flashed it
# Error cases
# * Did not find baud, there was new firmware, flashed it
#
# Other cases should "never" happen (and have not been observed):
# * Did not find baud, there was no new firmware
# The only known way to not find the baud is if pixhawk is stuck in
# its bootloader, which happens because flashing was interrupted,
# which means there is new firmware available.
# * Found baud, there was new firmware, error flashing it
# * Did not find baud, there was new firmware, error flashing it
# Should "never" fail to flash pixhawk when we try to.
# This should work for any of the three known-to-happen cases. For the
# error cases, running_version will be set to an empty dictionary, and
# write_version_file will write "unknown" for all the versions.
# get_version() can return None if the configuration is corrupt, but in
# that case we have far deeper problems (an md5 has just succeeded).
running_version = get_version()
logger.info("now running:")
for component in [
"build_version", "ardupilot_git_hash", "px4_git_hash",
"nuttx_git_hash"
]:
if component in running_version:
version = running_version[component]
else:
version = "unknown"
logger.info("%-20s %s", component, version)
write_version_file(running_version)
if "build_version" in running_version \
and running_version["build_version"] != "unknown":
logger.info("pixhawk status: ready")
print "pixhawk: running %s" % running_version["build_version"]
else:
print "pixhawk: ERROR checking version"
led.off()
end_us = clock.gettime_us(clock.CLOCK_MONOTONIC)
logger.debug("pixhawk initialization took %0.3f sec",
(end_us - start_us) / 1000000.0)