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

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

This commit is contained in:
Lorenz Meier 2014-02-10 10:08:34 +01:00
parent 0dc7f223ea ce83f450b8
commit 7baa78b113
11 changed files with 56 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/4001_quad_x
View File

@ -48,5 +48,5 @@ set MIXER FMU_quad_x
set PWM_OUTPUTS 1234
set PWM_RATE 400
set PWM_DISARMED 900
set PWM_MIN 1000
set PWM_MIN 1100
set PWM_MAX 2000

2
ROMFS/px4fmu_common/init.d/rc.logging
View File

@ -8,7 +8,7 @@ then
if hw_ver compare PX4FMU_V1
then
echo "Start sdlog2 at 50Hz"
sdlog2 start -r 50 -a -b 16 -t
sdlog2 start -r 50 -a -b 8 -t
else
echo "Start sdlog2 at 200Hz"
sdlog2 start -r 200 -a -b 16 -t

2
ROMFS/px4fmu_common/init.d/rcS
View File

@ -474,7 +474,7 @@ then
set MAV_TYPE 2
# Use mixer to detect vehicle type
if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ]
if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ]
then
set MAV_TYPE 13
fi

0
ROMFS/px4fmu_common/mixers/FMU_hex_+.mix → ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix
View File

0
ROMFS/px4fmu_common/mixers/FMU_hex_x.mix → ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix
View File

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

@ -236,7 +236,7 @@ private:
float land_slope_length;
float land_H1_virt;
float land_flare_alt_relative;
float land_thrust_lim_horizontal_distance;
float land_thrust_lim_alt_relative;
float land_heading_hold_horizontal_distance;
} _parameters; /**< local copies of interesting parameters */
@ -281,7 +281,7 @@ private:
param_t land_slope_length;
param_t land_H1_virt;
param_t land_flare_alt_relative;
param_t land_thrust_lim_horizontal_distance;
param_t land_thrust_lim_alt_relative;
param_t land_heading_hold_horizontal_distance;
} _parameter_handles; /**< handles for interesting parameters */
@ -434,7 +434,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_slope_length = param_find("FW_LND_SLLR");
_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
_parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST");
_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
@ -523,7 +523,7 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance));
param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
_l1_control.set_l1_damping(_parameters.l1_damping);
@ -558,7 +558,7 @@ FixedwingPositionControl::parameters_update()
}
/* Update the landing slope */
landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt);
landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _parameters.land_H1_virt);
/* Update and publish the navigation capabilities */
_nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
@ -836,6 +836,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
/* Horizontal landing control */
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
@ -846,7 +848,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (!land_noreturn_horizontal) {//set target_bearing in first occurrence
if (pos_sp_triplet.previous.valid) {
target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
target_bearing = bearing_lastwp_currwp;
} else {
target_bearing = _att.yaw;
}
@ -888,10 +890,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float airspeed_approach = 1.3f * _parameters.airspeed_min;
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement);
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement);
float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
@ -903,7 +904,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* kill the throttle if param requests it */
throttle_max = _parameters.throttle_max;
if (wp_distance < landingslope.motor_lim_horizontal_distance() || land_motor_lim) {
if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
if (!land_motor_lim) {
land_motor_lim = true;
@ -912,7 +913,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt);
float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
/* avoid climbout */
if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)

2
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -172,5 +172,5 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f);
PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
PARAM_DEFINE_FLOAT(FW_LND_TLDIST, 30.0f);
PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);

22
src/modules/fw_pos_control_l1/landingslope.cpp
View File

@ -48,13 +48,13 @@
void Landingslope::update(float landing_slope_angle_rad,
float flare_relative_alt,
float motor_lim_horizontal_distance,
float motor_lim_relative_alt,
float H1_virt)
{
_landing_slope_angle_rad = landing_slope_angle_rad;
_flare_relative_alt = flare_relative_alt;
_motor_lim_horizontal_distance = motor_lim_horizontal_distance;
_motor_lim_relative_alt = motor_lim_relative_alt;
_H1_virt = H1_virt;
calculateSlopeValues();
@ -74,9 +74,21 @@ float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_
return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
}
float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude)
float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
{
return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude);
else
return wp_altitude;
}
float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
{
/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
else
return wp_landing_altitude;
}

21
src/modules/fw_pos_control_l1/landingslope.h
View File

@ -49,7 +49,7 @@ private:
/* see Documentation/fw_landing.png for a plot of the landing slope */
float _landing_slope_angle_rad; /**< phi in the plot */
float _flare_relative_alt; /**< h_flare,rel in the plot */
float _motor_lim_horizontal_distance;
float _motor_lim_relative_alt;
float _H1_virt; /**< H1 in the plot */
float _H0; /**< h_flare,rel + H1 in the plot */
float _d1; /**< d1 in the plot */
@ -63,7 +63,18 @@ public:
Landingslope() {}
~Landingslope() {}
float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude);
/**
*
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude);
/**
*
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
* Performs check if aircraft is in front of waypoint to avoid climbout
*/
float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude);
/**
*
@ -85,17 +96,17 @@ public:
}
float getFlareCurveAltitude(float wp_distance, float wp_altitude);
float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
void update(float landing_slope_angle_rad,
float flare_relative_alt,
float motor_lim_horizontal_distance,
float motor_lim_relative_alt,
float H1_virt);
inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}
inline float flare_relative_alt() {return _flare_relative_alt;}
inline float motor_lim_horizontal_distance() {return _motor_lim_horizontal_distance;}
inline float motor_lim_relative_alt() {return _motor_lim_relative_alt;}
inline float H1_virt() {return _H1_virt;}
inline float H0() {return _H0;}
inline float d1() {return _d1;}

6
src/modules/systemlib/mixer/mixer_load.c
View File

@ -52,7 +52,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
/* open the mixer definition file */
fp = fopen(fname, "r");
if (fp == NULL) {
return 1;
warnx("file not found");
return -1;
}
/* read valid lines from the file into a buffer */
@ -88,7 +89,8 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
/* if the line is too long to fit in the buffer, bail */
if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
return 1;
warnx("line too long");
return -1;
}
/* add the line to the buffer */

3
src/systemcmds/mixer/mixer.cpp
View File

@ -102,7 +102,8 @@ load(const char *devname, const char *fname)
if (ioctl(dev, MIXERIOCRESET, 0))
err(1, "can't reset mixers on %s", devname);
load_mixer_file(fname, &buf[0], sizeof(buf));
if (load_mixer_file(fname, &buf[0], sizeof(buf)) < 0)
err(1, "can't load mixer: %s", fname);
/* XXX pass the buffer to the device */
int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);