dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' into real_life_timeouts

This commit is contained in:
Lorenz Meier 2014-07-29 17:31:09 +02:00
parent 8e12d79ef4 49c877c3e6
commit 3257cd9151
11 changed files with 271 additions and 170 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

207
Tools/fetch_file.py Normal file
View File

@ -0,0 +1,207 @@
#!/usr/bin/env python
############################################################################
#
# Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
"""Fetch files via nsh console
Usage: python fetch_file.py [-l] [-f] [-d device] [-s speed] [-o out_path] path
\t-l\tList files
\t-f\tOverwrite existing files
\t-d\tSerial device
\t-s\tSerial baudrate
\t-o\tOutput path
\tpath\tPath to list/fetch, if ends with "/" then directory will be fetched recursively"""
__author__ = "Anton Babushkin"
__version__ = "1.1"
import serial, time, sys, os
def _wait_for_string(ser, s, timeout):
t0 = time.time()
buf = []
res = []
while (True):
c = ser.read()
buf.append(c)
if len(buf) > len(s):
res.append(buf.pop(0))
if "".join(buf) == s:
break
if timeout > 0.0 and time.time() - t0 > timeout:
raise Exception("Timeout while waiting for: " + s)
return "".join(res)
def _exec_cmd(ser, cmd, timeout):
ser.write(cmd + "\n")
ser.flush()
_wait_for_string(ser, cmd + "\r\n", timeout)
return _wait_for_string(ser, "nsh> \x1b[K", timeout)
def _ls_dir_raw(ser, dir, timeout):
return _exec_cmd(ser, "ls -l " + dir, timeout)
def _ls_dir(ser, dir, timeout):
res = []
for line in _ls_dir_raw(ser, dir, timeout).splitlines():
if line == dir + ":":
continue
if line.startswith("nsh: ls: no such directory:"):
raise Exception("No such file: " + dir)
res.append((line[20:], int(line[11:19].strip()), line[1] == "d"))
return res
def _get_file(ser, fn, fn_out, force, timeout):
print "Get %s:" % fn,
if not force:
# Check if file already exists with the same size
try:
os.stat(fn_out)
except:
pass
else:
print "already fetched, skip"
return
cmd = "dumpfile " + fn
ser.write(cmd + "\n")
ser.flush()
_wait_for_string(ser, cmd + "\r\n", timeout)
res = _wait_for_string(ser, "\n", timeout)
if res.startswith("OK"):
# Got correct responce, open temp file
fn_out_part = fn_out + ".part"
fout = open(fn_out_part, "wb")
size = int(res.split()[1])
sys.stdout.write(" [%i bytes] " % size)
n = 0
while (n < size):
buf = ser.read(min(size - n, 8192))
n += len(buf)
sys.stdout.write(".")
sys.stdout.flush()
fout.write(buf)
print " done"
fout.close()
os.rename(fn_out_part, fn_out)
else:
raise Exception("Error reading file")
_wait_for_string(ser, "nsh> \x1b[K", timeout)
def _get_files_in_dir(ser, path, path_out, force, timeout):
try:
os.mkdir(path_out)
except:
pass
for fn in _ls_dir(ser, path, timeout):
path_fn = os.path.join(path, fn[0])
path_fn_out = os.path.join(path_out, fn[0])
if fn[2]:
_get_files_in_dir(ser, path_fn[:-1], path_fn_out[:-1], force, timeout)
else:
_get_file(ser, path_fn, path_fn_out, force, timeout)
def _usage():
print """Usage: python fetch_file.py [-l] [-f] [-d device] [-s speed] [-o out_path] path
\t-l\tList files
\t-f\tOverwrite existing files
\t-d\tSerial device
\t-s\tSerial baudrate
\t-o\tOutput path
\tpath\tPath to list/fetch, if ends with "/" then directory will be fetched recursively"""
def _main():
dev = "/dev/tty.usbmodem1"
speed = "57600"
cmd = "get"
path = None
path_out = None
force = False
opt = None
for arg in sys.argv[1:]:
if opt != None:
if opt == "d":
dev = arg
elif opt == "s":
speed = arg
elif opt == "o":
path_out = arg
opt = None
else:
if arg == "-l":
cmd = "ls"
elif arg == "-f":
force = True
elif arg == "-d":
opt = "d"
elif arg == "-s":
opt = "s"
elif arg == "-o":
opt = "o"
elif path == None:
path = arg
if path == None:
_usage()
exit(0)
# Connect to serial port
ser = serial.Serial(dev, speed, timeout=0.2)
timeout = 1.0
try:
if cmd == "ls":
# List directory
print _ls_dir_raw(ser, path, timeout)
elif cmd == "get":
# Get file(s)
if path.endswith("/"):
# Get all files from directory recursively
if path_out == None:
path_out = os.path.split(path[:-1])[1]
_get_files_in_dir(ser, path[:-1], path_out, force, timeout)
else:
# Get one file
if path_out == None:
path_out = os.path.split(path)[1]
_get_file(ser, path, os.path.split(path)[1], force, timeout)
except Exception as e:
print e
ser.close()
if __name__ == "__main__":
_main()

133
Tools/fetch_log.py
View File

@ -1,133 +0,0 @@
#!/usr/bin/env python
############################################################################
#
# Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Log fetcher
#
# Print list of logs:
# python fetch_log.py
#
# Fetch log:
# python fetch_log.py sess001/log001.bin
#
import serial, time, sys, os
def wait_for_string(ser, s, timeout=1.0, debug=False):
t0 = time.time()
buf = []
res = []
n = 0
while (True):
c = ser.read()
if debug:
sys.stderr.write(c)
buf.append(c)
if len(buf) > len(s):
res.append(buf.pop(0))
n += 1
if n % 10000 == 0:
sys.stderr.write(str(n) + "\n")
if "".join(buf) == s:
break
if timeout > 0.0 and time.time() - t0 > timeout:
raise Exception("Timeout while waiting for: " + s)
return "".join(res)
def exec_cmd(ser, cmd, timeout):
ser.write(cmd + "\n")
ser.flush()
wait_for_string(ser, cmd + "\r\n", timeout)
return wait_for_string(ser, "nsh> \x1b[K", timeout)
def ls_dir(ser, dir, timeout=1.0):
res = []
for line in exec_cmd(ser, "ls -l " + dir, timeout).splitlines()[1:]:
res.append((line[20:], int(line[11:19].strip()), line[1] == "d"))
return res
def list_logs(ser):
logs_dir = "/fs/microsd/log"
res = []
for d in ls_dir(ser, logs_dir):
if d[2]:
sess_dir = d[0][:-1]
for f in ls_dir(ser, logs_dir + "/" + sess_dir):
log_file = f[0]
log_size = f[1]
res.append(sess_dir + "/" + log_file + "\t" + str(log_size))
return "\n".join(res)
def fetch_log(ser, fn, timeout):
cmd = "dumpfile " + fn
ser.write(cmd + "\n")
ser.flush()
wait_for_string(ser, cmd + "\r\n", timeout, True)
res = wait_for_string(ser, "\n", timeout, True)
data = []
if res.startswith("OK"):
size = int(res.split()[1])
n = 0
print "Reading data:"
while (n < size):
buf = ser.read(min(size - n, 8192))
data.append(buf)
n += len(buf)
sys.stdout.write(".")
sys.stdout.flush()
print
else:
raise Exception("Error reading log")
wait_for_string(ser, "nsh> \x1b[K", timeout)
return "".join(data)
def main():
dev = "/dev/tty.usbmodem1"
ser = serial.Serial(dev, "115200", timeout=0.2)
if len(sys.argv) < 2:
print list_logs(ser)
else:
log_file = sys.argv[1]
data = fetch_log(ser, "/fs/microsd/log/" + log_file, 1.0)
try:
os.mkdir(log_file.split("/")[0])
except:
pass
fout = open(log_file, "wb")
fout.write(data)
fout.close()
ser.close()
if __name__ == "__main__":
main()

8
src/drivers/px4io/px4io_serial.cpp
View File

@ -157,6 +157,10 @@ private:
perf_counter_t _pc_idle;
perf_counter_t _pc_badidle;
/* do not allow top copying this class */
PX4IO_serial(PX4IO_serial &);
PX4IO_serial& operator = (const PX4IO_serial &);
};
IOPacket PX4IO_serial::_dma_buffer;
@ -173,7 +177,9 @@ PX4IO_serial::PX4IO_serial() :
_tx_dma(nullptr),
_rx_dma(nullptr),
_rx_dma_status(_dma_status_inactive),
_pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")),
_bus_semaphore(SEM_INITIALIZER(0)),
_completion_semaphore(SEM_INITIALIZER(0)),
_pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")),
_pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")),
_pc_retries(perf_alloc(PC_COUNT, "io_retries ")),
_pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")),

29
src/drivers/px4io/px4io_uploader.cpp
View File

@ -65,7 +65,8 @@
PX4IO_Uploader::PX4IO_Uploader() :
_io_fd(-1),
_fw_fd(-1)
_fw_fd(-1),
bl_rev(0)
{
}
@ -245,7 +246,7 @@ PX4IO_Uploader::upload(const char *filenames[])
}
int
PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
PX4IO_Uploader::recv_byte_with_timeout(uint8_t *c, unsigned timeout)
{
struct pollfd fds[1];
@ -262,19 +263,19 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
return -ETIMEDOUT;
}
read(_io_fd, &c, 1);
read(_io_fd, c, 1);
#ifdef UDEBUG
log("recv 0x%02x", c);
log("recv_bytes 0x%02x", c);
#endif
return OK;
}
int
PX4IO_Uploader::recv(uint8_t *p, unsigned count)
PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count)
{
int ret;
int ret = OK;
while (count--) {
ret = recv(*p++, 5000);
ret = recv_byte_with_timeout(p++, 5000);
if (ret != OK)
break;
@ -289,10 +290,10 @@ PX4IO_Uploader::drain()
int ret;
do {
// the small recv timeout here is to allow for fast
// the small recv_bytes timeout here is to allow for fast
// drain when rebooting the io board for a forced
// update of the fw without using the safety switch
ret = recv(c, 40);
ret = recv_byte_with_timeout(&c, 40);
#ifdef UDEBUG
if (ret == OK) {
@ -331,12 +332,12 @@ PX4IO_Uploader::get_sync(unsigned timeout)
uint8_t c[2];
int ret;
ret = recv(c[0], timeout);
ret = recv_byte_with_timeout(c, timeout);
if (ret != OK)
return ret;
ret = recv(c[1], timeout);
ret = recv_byte_with_timeout(c + 1, timeout);
if (ret != OK)
return ret;
@ -372,7 +373,7 @@ PX4IO_Uploader::get_info(int param, uint32_t &val)
send(param);
send(PROTO_EOC);
ret = recv((uint8_t *)&val, sizeof(val));
ret = recv_bytes((uint8_t *)&val, sizeof(val));
if (ret != OK)
return ret;
@ -513,7 +514,7 @@ PX4IO_Uploader::verify_rev2(size_t fw_size)
for (ssize_t i = 0; i < count; i++) {
uint8_t c;
ret = recv(c, 5000);
ret = recv_byte_with_timeout(&c, 5000);
if (ret != OK) {
log("%d: got %d waiting for bytes", sent + i, ret);
@ -600,7 +601,7 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
send(PROTO_GET_CRC);
send(PROTO_EOC);
ret = recv((uint8_t*)(&crc), sizeof(crc));
ret = recv_bytes((uint8_t*)(&crc), sizeof(crc));
if (ret != OK) {
log("did not receive CRC checksum");

8
src/drivers/px4io/uploader.h
View File

@ -74,19 +74,19 @@ private:
INFO_BOARD_REV = 3, /**< board revision */
INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */
PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */
PROG_MULTI_MAX = 248, /**< protocol max is 255, must be multiple of 4 */
};
int _io_fd;
int _fw_fd;
uint32_t bl_rev; /**< bootloader revision */
uint32_t bl_rev; /**< bootloader revision */
void log(const char *fmt, ...);
int recv(uint8_t &c, unsigned timeout);
int recv(uint8_t *p, unsigned count);
int recv_byte_with_timeout(uint8_t *c, unsigned timeout);
int recv_bytes(uint8_t *p, unsigned count);
void drain();
int send(uint8_t c);
int send(uint8_t *p, unsigned count);

4
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
View File

@ -3028,6 +3028,10 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err)
current_ekf_state.states[i] = states[i];
}
current_ekf_state.n_states = n_states;
current_ekf_state.onGround = onGround;
current_ekf_state.staticMode = staticMode;
current_ekf_state.useCompass = useCompass;
current_ekf_state.useAirspeed = useAirspeed;
memcpy(err, &current_ekf_state, sizeof(*err));

4
src/modules/ekf_att_pos_estimator/estimator_utilities.h
View File

@ -68,6 +68,10 @@ struct ekf_status_report {
bool posTimeout;
bool hgtTimeout;
bool imuTimeout;
bool onGround;
bool staticMode;
bool useCompass;
bool useAirspeed;
uint32_t velFailTime;
uint32_t posFailTime;
uint32_t hgtFailTime;

7
src/modules/mavlink/mavlink_main.cpp
View File

@ -483,7 +483,12 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
Mavlink *inst;
LL_FOREACH(_mavlink_instances, inst) {
if (inst != self) {
inst->pass_message(msg);
/* if not in normal mode, we are an onboard link
* onboard links should only pass on messages from the same system ID */
if(!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) {
inst->pass_message(msg);
}
}
}
}

3
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -241,7 +241,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
} else {
if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
warnx("ignoring CMD spoofed with same SYS/COMP ID");
warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
mavlink_system.sysid, mavlink_system.compid);
return;
}

36
src/modules/navigator/mission.cpp
View File

@ -314,30 +314,36 @@ Mission::set_mission_items()
/* set previous position setpoint to current */
set_previous_pos_setpoint();
/* get home distance state */
bool home_dist_ok = check_dist_1wp();
/* the home dist check provides user feedback, so we initialize it to this */
bool user_feedback_done = !home_dist_ok;
/* try setting onboard mission item */
if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running");
mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
}
_mission_type = MISSION_TYPE_ONBOARD;
/* try setting offboard mission item */
} else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) {
} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running");
mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
}
_mission_type = MISSION_TYPE_OFFBOARD;
} else {
/* no mission available, switch to loiter */
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: mission finished");
} else {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: no mission available");
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
} else if (!user_feedback_done) {
/* only tell users that we got no mission if there has not been any
* better, more specific feedback yet
*/
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available");
}
_mission_type = MISSION_TYPE_NONE;
@ -397,7 +403,7 @@ Mission::set_mission_items()
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
}
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
@ -483,7 +489,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR waypoint could not be read");
"ERROR waypoint could not be read");
return false;
}
@ -502,7 +508,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR DO JUMP waypoint could not be written");
"ERROR DO JUMP waypoint could not be written");
return false;
}
}
@ -511,8 +517,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
} else {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: DO JUMP repetitions completed");
mavlink_log_critical(_navigator->get_mavlink_fd(),
"DO JUMP repetitions completed");
/* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index_ptr)++;
}
@ -526,7 +532,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
/* we have given up, we don't want to cycle forever */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR DO JUMP is cycling, giving up");
"ERROR DO JUMP is cycling, giving up");
return false;
}

2
src/systemcmds/nshterm/module.mk
View File

@ -38,4 +38,4 @@
MODULE_COMMAND = nshterm
SRCS = nshterm.c
MODULE_STACKSIZE = 1200
MODULE_STACKSIZE = 1400