fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware into gpio_led_fmuv2

Browse Source
This commit is contained in:
Lorenz Meier 2014-03-12 20:35:20 +01:00
commit caef8d115c
24 changed files with 911 additions and 628 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

133
Tools/fetch_log.py Normal file
View File

@ -0,0 +1,133 @@
#!/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()

83
Tools/px_romfs_pruner.py Normal file
View File

@ -0,0 +1,83 @@
#!/usr/bin/env python
############################################################################
#
# Copyright (C) 2014 PX4 Development Team. All rights reserved.
# Author: Julian Oes <joes@student.ethz.ch>
# 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.
#
############################################################################
"""
px_romfs_pruner.py:
Delete all comments and newlines before ROMFS is converted to an image
"""
from __future__ import print_function
import argparse
import os
def main():
# Parse commandline arguments
parser = argparse.ArgumentParser(description="ROMFS pruner.")
parser.add_argument('--folder', action="store", help="ROMFS scratch folder.")
args = parser.parse_args()
print("Pruning ROMFS files.")
# go through
for (root, dirs, files) in os.walk(args.folder):
for file in files:
# only prune text files
if ".zip" in file or ".bin" in file:
continue
file_path = os.path.join(root, file)
# read file line by line
pruned_content = ""
with open(file_path, "r") as f:
for line in f:
# handle mixer files differently than startup files
if file_path.endswith(".mix"):
if line.startswith(("Z:", "M:", "R: ", "O:", "S:")):
pruned_content += line
else:
if not line.isspace() and not line.strip().startswith("#"):
pruned_content += line
# overwrite old scratch file
with open(file_path, "w") as f:
f.write(pruned_content)
if __name__ == '__main__':
main()

1
makefiles/config_px4fmu-v1_default.mk
View File

@ -56,6 +56,7 @@ MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
MODULES += systemcmds/hw_ver
MODULES += systemcmds/dumpfile
#
# General system control

1
makefiles/config_px4fmu-v2_default.mk
View File

@ -63,6 +63,7 @@ MODULES += systemcmds/config
MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
MODULES += systemcmds/hw_ver
MODULES += systemcmds/dumpfile
#
# General system control

4
makefiles/firmware.mk
View File

@ -355,6 +355,9 @@ ROMFS_OBJ = $(ROMFS_CSRC:.c=.o)
LIBS += $(ROMFS_OBJ)
LINK_DEPS += $(ROMFS_OBJ)
# Remove all comments from startup and mixer files
ROMFS_PRUNER = $(PX4_BASE)/Tools/px_romfs_pruner.py
# Turn the ROMFS image into an object file
$(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS)
$(call BIN_TO_OBJ,$<,$@,romfs_img)
@ -372,6 +375,7 @@ ifneq ($(ROMFS_EXTRA_FILES),)
$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras
$(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras
endif
$(Q) $(PYTHON) -u $(ROMFS_PRUNER) --folder $(ROMFS_SCRATCH)
EXTRA_CLEANS += $(ROMGS_OBJ) $(ROMFS_IMG)

6
nuttx-configs/px4fmu-v1/nsh/defconfig
View File

@ -417,7 +417,7 @@ CONFIG_PREALLOC_TIMERS=50
#
# Stack and heap information
#
CONFIG_IDLETHREAD_STACKSIZE=6000
CONFIG_IDLETHREAD_STACKSIZE=4096
CONFIG_USERMAIN_STACKSIZE=4096
CONFIG_PTHREAD_STACK_MIN=512
CONFIG_PTHREAD_STACK_DEFAULT=2048
@ -720,11 +720,11 @@ CONFIG_SCHED_WORKQUEUE=y
CONFIG_SCHED_HPWORK=y
CONFIG_SCHED_WORKPRIORITY=192
CONFIG_SCHED_WORKPERIOD=5000
CONFIG_SCHED_WORKSTACKSIZE=4000
CONFIG_SCHED_WORKSTACKSIZE=2048
CONFIG_SCHED_LPWORK=y
CONFIG_SCHED_LPWORKPRIORITY=50
CONFIG_SCHED_LPWORKPERIOD=50000
CONFIG_SCHED_LPWORKSTACKSIZE=4000
CONFIG_SCHED_LPWORKSTACKSIZE=2048
# CONFIG_LIB_KBDCODEC is not set
# CONFIG_LIB_SLCDCODEC is not set

17
src/lib/launchdetection/CatapultLaunchMethod.cpp
View File

@ -41,12 +41,16 @@
#include "CatapultLaunchMethod.h"
#include <systemlib/err.h>
CatapultLaunchMethod::CatapultLaunchMethod() :
namespace launchdetection
{
CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
SuperBlock(parent, "CAT"),
last_timestamp(hrt_absolute_time()),
integrator(0.0f),
launchDetected(false),
threshold_accel(NULL, "LAUN_CAT_A", false),
threshold_time(NULL, "LAUN_CAT_T", false)
threshold_accel(this, "A"),
threshold_time(this, "T")
{
}
@ -83,14 +87,11 @@ bool CatapultLaunchMethod::getLaunchDetected()
return launchDetected;
}
void CatapultLaunchMethod::updateParams()
{
threshold_accel.update();
threshold_time.update();
}
void CatapultLaunchMethod::reset()
{
integrator = 0.0f;
launchDetected = false;
}
}

11
src/lib/launchdetection/CatapultLaunchMethod.h
View File

@ -44,17 +44,20 @@
#include "LaunchMethod.h"
#include <drivers/drv_hrt.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
class CatapultLaunchMethod : public LaunchMethod
namespace launchdetection
{
class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock
{
public:
CatapultLaunchMethod();
CatapultLaunchMethod(SuperBlock *parent);
~CatapultLaunchMethod();
void update(float accel_x);
bool getLaunchDetected();
void updateParams();
void reset();
private:
@ -68,3 +71,5 @@ private:
};
#endif /* CATAPULTLAUNCHMETHOD_H_ */
}

18
src/lib/launchdetection/LaunchDetector.cpp
View File

@ -42,12 +42,16 @@
#include "CatapultLaunchMethod.h"
#include <systemlib/err.h>
namespace launchdetection
{
LaunchDetector::LaunchDetector() :
launchdetection_on(NULL, "LAUN_ALL_ON", false),
throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
SuperBlock(NULL, "LAUN"),
launchdetection_on(this, "ALL_ON"),
throttlePreTakeoff(this, "THR_PRE")
{
/* init all detectors */
launchMethods[0] = new CatapultLaunchMethod();
launchMethods[0] = new CatapultLaunchMethod(this);
/* update all parameters of all detectors */
@ -87,12 +91,4 @@ bool LaunchDetector::getLaunchDetected()
return false;
}
void LaunchDetector::updateParams() {
launchdetection_on.update();
throttlePreTakeoff.update();
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
launchMethods[i]->updateParams();
}
}

9
src/lib/launchdetection/LaunchDetector.h
View File

@ -45,10 +45,13 @@
#include <stdint.h>
#include "LaunchMethod.h"
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
class __EXPORT LaunchDetector
namespace launchdetection
{
class __EXPORT LaunchDetector : public control::SuperBlock
{
public:
LaunchDetector();
@ -57,7 +60,6 @@ public:
void update(float accel_x);
bool getLaunchDetected();
void updateParams();
bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
@ -72,5 +74,6 @@ private:
};
}
#endif // LAUNCHDETECTOR_H

7
src/lib/launchdetection/LaunchMethod.h
View File

@ -41,15 +41,20 @@
#ifndef LAUNCHMETHOD_H_
#define LAUNCHMETHOD_H_
namespace launchdetection
{
class LaunchMethod
{
public:
virtual void update(float accel_x) = 0;
virtual bool getLaunchDetected() = 0;
virtual void updateParams() = 0;
virtual void reset() = 0;
protected:
private:
};
}
#endif /* LAUNCHMETHOD_H_ */

3
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -477,6 +477,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
dt = 0.005f;
parameters_update(&ekf_param_handles, &ekf_params);
/* update mag declination rotation matrix */
R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
x_aposteriori_k[0] = z_k[0];
x_aposteriori_k[1] = z_k[1];
x_aposteriori_k[2] = z_k[2];

56
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -166,6 +166,15 @@ private:
float airspeed_min;
float airspeed_trim;
float airspeed_max;
float trim_roll;
float trim_pitch;
float trim_yaw;
float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */
float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */
float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */
float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */
} _parameters; /**< local copies of interesting parameters */
struct {
@ -197,6 +206,12 @@ private:
param_t airspeed_min;
param_t airspeed_trim;
param_t airspeed_max;
param_t trim_roll;
param_t trim_pitch;
param_t trim_yaw;
param_t rollsp_offset_deg;
param_t pitchsp_offset_deg;
} _parameter_handles; /**< handles for interesting parameters */
@ -335,6 +350,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN");
_parameter_handles.trim_roll = param_find("TRIM_ROLL");
_parameter_handles.trim_pitch = param_find("TRIM_PITCH");
_parameter_handles.trim_yaw = param_find("TRIM_YAW");
_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
/* fetch initial parameter values */
parameters_update();
}
@ -395,6 +416,15 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.tconst);
_pitch_ctrl.set_k_p(_parameters.p_p);
@ -648,13 +678,13 @@ FixedwingAttitudeControl::task_main()
float airspeed_scaling = _parameters.airspeed_trim / airspeed;
//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
float roll_sp = 0.0f;
float pitch_sp = 0.0f;
float roll_sp = _parameters.rollsp_offset_rad;
float pitch_sp = _parameters.pitchsp_offset_rad;
float throttle_sp = 0.0f;
if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
roll_sp = _att_sp.roll_body;
pitch_sp = _att_sp.pitch_body;
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
@ -670,9 +700,13 @@ FixedwingAttitudeControl::task_main()
* With this mapping the stick angle is a 1:1 representation of
* the commanded attitude. If more than 45 degrees are desired,
* a scaling parameter can be applied to the remote.
*
* The trim gets subtracted here from the manual setpoint to get
* the intended attitude setpoint. Later, after the rate control step the
* trim is added again to the control signal.
*/
roll_sp = _manual.roll * 0.75f;
pitch_sp = _manual.pitch * 0.75f;
roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad;
pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad;
throttle_sp = _manual.throttle;
_actuators.control[4] = _manual.flaps;
@ -685,7 +719,7 @@ FixedwingAttitudeControl::task_main()
att_sp.timestamp = hrt_absolute_time();
att_sp.roll_body = roll_sp;
att_sp.pitch_body = pitch_sp;
att_sp.yaw_body = 0.0f;
att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
att_sp.thrust = throttle_sp;
/* lazily publish the setpoint only once available */
@ -719,12 +753,12 @@ FixedwingAttitudeControl::task_main()
speed_body_u, speed_body_v, speed_body_w,
_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
/* Run attitude RATE controllers which need the desired attitudes from above */
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
_att.rollspeed, _att.yawspeed,
_yaw_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f;
_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!isfinite(roll_u)) {
warnx("roll_u %.4f", roll_u);
}
@ -733,7 +767,7 @@ FixedwingAttitudeControl::task_main()
_att.pitchspeed, _att.yawspeed,
_yaw_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f;
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!isfinite(pitch_u)) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body);
@ -743,7 +777,7 @@ FixedwingAttitudeControl::task_main()
_att.pitchspeed, _att.yawspeed,
_pitch_ctrl.get_desired_rate(),
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f;
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
if (!isfinite(yaw_u)) {
warnx("yaw_u %.4f", yaw_u);
}

10
src/modules/fw_att_control/fw_att_control_params.c
View File

@ -176,3 +176,13 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
// @Description If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively
// @Range 0.0 to 30
PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
// @DisplayName Roll Setpoint Offset
// @Description An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe
// @Range -90.0 to 90.0
PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
// @DisplayName Pitch Setpoint Offset
// @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe
// @Range -90.0 to 90.0
PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);

11
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -186,7 +186,7 @@ private:
float target_bearing;
/* Launch detection */
LaunchDetector launchDetector;
launchdetection::LaunchDetector launchDetector;
/* throttle and airspeed states */
float _airspeed_error; ///< airspeed error to setpoint in m/s
@ -350,12 +350,12 @@ private:
/*
* Reset takeoff state
*/
int reset_takeoff_state();
void reset_takeoff_state();
/*
* Reset landing state
*/
int reset_landing_state();
void reset_landing_state();
};
namespace l1_control
@ -989,6 +989,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else {
/* no takeoff detection --> fly */
launch_detected = true;
warnx("launchdetection off");
}
}
@ -1311,14 +1312,14 @@ FixedwingPositionControl::task_main()
_exit(0);
}
int FixedwingPositionControl::reset_takeoff_state()
void FixedwingPositionControl::reset_takeoff_state()
{
launch_detected = false;
usePreTakeoffThrust = false;
launchDetector.reset();
}
int FixedwingPositionControl::reset_landing_state()
void FixedwingPositionControl::reset_landing_state()
{
land_noreturn_horizontal = false;
land_noreturn_vertical = false;

11
src/modules/navigator/geofence.cpp
View File

@ -55,11 +55,13 @@
#endif
static const int ERROR = -1;
Geofence::Geofence() : _fence_pub(-1),
Geofence::Geofence() :
SuperBlock(NULL, "GF"),
_fence_pub(-1),
_altitude_min(0),
_altitude_max(0),
_verticesCount(0),
param_geofence_on(NULL, "GF_ON", false)
param_geofence_on(this, "ON")
{
/* Load initial params */
updateParams();
@ -292,8 +294,3 @@ int Geofence::clearDm()
{
dm_clear(DM_KEY_FENCE_POINTS);
}
void Geofence::updateParams()
{
param_geofence_on.update();
}

6
src/modules/navigator/geofence.h
View File

@ -41,11 +41,13 @@
#define GEOFENCE_H_
#include <uORB/topics/fence.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt"
class Geofence {
class Geofence : public control::SuperBlock
{
private:
orb_advert_t _fence_pub; /**< publish fence topic */
@ -85,8 +87,6 @@ public:
int loadFromFile(const char *filename);
bool isEmpty() {return _verticesCount == 0;}
void updateParams();
};

5
src/modules/position_estimator_inav/inertial_filter.c
View File

@ -15,10 +15,7 @@ void inertial_filter_predict(float dt, float x[3])
void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
{
float ewdt = w * dt;
if (ewdt > 1.0f)
ewdt = 1.0f; // prevent over-correcting
ewdt *= e;
float ewdt = e * w * dt;
x[i] += ewdt;
if (i == 0) {

2
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -623,7 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
dt = fmaxf(fminf(0.02, dt), 0.005);
dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms
t_prev = t;
/* use GPS if it's valid and reference position initialized */

3
src/modules/sdlog2/logbuffer.c
View File

@ -74,8 +74,9 @@ bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
// bytes available to write
int available = lb->read_ptr - lb->write_ptr - 1;
if (available < 0)
if (available < 0) {
available += lb->size;
}
if (size > available) {
// buffer overflow

971
src/modules/sdlog2/sdlog2.c
View File

File diff suppressed because it is too large Load Diff

14
src/modules/sdlog2/sdlog2_messages.h
View File

@ -267,13 +267,13 @@ struct log_DIST_s {
/* --- TELE - TELEMETRY STATUS --- */
#define LOG_TELE_MSG 22
struct log_TELE_s {
uint8_t rssi;
uint8_t remote_rssi;
uint8_t noise;
uint8_t remote_noise;
uint16_t rxerrors;
uint16_t fixed;
uint8_t txbuf;
uint8_t rssi;
uint8_t remote_rssi;
uint8_t noise;
uint8_t remote_noise;
uint16_t rxerrors;
uint16_t fixed;
uint8_t txbuf;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/

116
src/systemcmds/dumpfile/dumpfile.c Normal file
View File

@ -0,0 +1,116 @@
/****************************************************************************
*
* Copyright (c) 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.
*
****************************************************************************/
/**
* @file dumpfile.c
*
* Dump file utility. Prints file size and contents in binary mode (don't replace LF with CR LF) to stdout.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <fcntl.h>
#include <termios.h>
#include <systemlib/err.h>
__EXPORT int dumpfile_main(int argc, char *argv[]);
int
dumpfile_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "usage: dumpfile <filename>");
}
/* open input file */
FILE *f;
f = fopen(argv[1], "r");
if (f == NULL) {
printf("ERROR\n");
exit(1);
}
/* get file size */
fseek(f, 0L, SEEK_END);
int size = ftell(f);
fseek(f, 0L, SEEK_SET);
printf("OK %d\n", size);
/* configure stdout */
int out = fileno(stdout);
struct termios tc;
struct termios tc_old;
tcgetattr(out, &tc);
/* save old terminal attributes to restore it later on exit */
memcpy(&tc_old, &tc, sizeof(tc));
/* don't add CR on each LF*/
tc.c_oflag &= ~ONLCR;
if (tcsetattr(out, TCSANOW, &tc) < 0) {
warnx("ERROR setting stdout attributes");
exit(1);
}
char buf[512];
int nread;
/* dump file */
while ((nread = fread(buf, 1, sizeof(buf), f)) > 0) {
if (write(out, buf, nread) <= 0) {
warnx("error dumping file");
break;
}
}
fsync(out);
fclose(f);
/* restore old terminal attributes */
if (tcsetattr(out, TCSANOW, &tc_old) < 0) {
warnx("ERROR restoring stdout attributes");
exit(1);
}
return OK;
}

41
src/systemcmds/dumpfile/module.mk Normal file
View File

@ -0,0 +1,41 @@
############################################################################
#
# Copyright (c) 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.
#
############################################################################
#
# Dump file utility
#
MODULE_COMMAND = dumpfile
SRCS = dumpfile.c
MAXOPTIMIZATION = -Os