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 remote-tracking branch 'upstream/master' into forcefail

Browse Source
This commit is contained in:
Thomas Gubler 2014-07-19 12:44:50 +02:00
commit d7e1502a26
17 changed files with 72 additions and 279 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
View File

@ -26,15 +26,6 @@ then
param set FW_RR_P 0.1
param set FW_R_LIM 45
param set FW_R_RMAX 0
param set FW_T_CLMB_MAX 5
param set FW_T_HRATE_P 0.02
param set FW_T_PTCH_DAMP 0
param set FW_T_RLL2THR 15
param set FW_T_SINK_MAX 5
param set FW_T_SINK_MIN 2
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 3
param set FW_T_VERT_ACC 7
param set FW_YR_FF 0.0
param set FW_YR_I 0
param set FW_YR_IMAX 0.2

4
ROMFS/px4fmu_common/init.d/3031_phantom
View File

@ -30,10 +30,6 @@ then
param set FW_RR_P 0.08
param set FW_R_LIM 50
param set FW_R_RMAX 0
param set FW_T_HRATE_P 0.01
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
fi
set MIXER phantom

4
ROMFS/px4fmu_common/init.d/3032_skywalker_x5
View File

@ -30,10 +30,6 @@ then
param set FW_RR_P 0.03
param set FW_R_LIM 60
param set FW_R_RMAX 0
param set FW_T_HRATE_P 0.01
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
fi
set MIXER FMU_X5

4
ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
View File

@ -33,10 +33,6 @@ then
param set FW_RR_P 0.03
param set FW_R_LIM 60
param set FW_R_RMAX 0
param set FW_T_HRATE_P 0.01
param set FW_T_RLL2THR 15
param set FW_T_SRATE_P 0.01
param set FW_T_TIME_CONST 5
fi
set MIXER FMU_Q

13
src/modules/controllib/blocks.cpp
View File

@ -293,7 +293,18 @@ int blockIntegralTrapTest()
float BlockDerivative::update(float input)
{
float output = _lowPass.update((input - getU()) / getDt());
float output;
if (_initialized) {
output = _lowPass.update((input - getU()) / getDt());
} else {
// if this is the first call to update
// we have no valid derivative
// and so we use the assumption the
// input value is not changing much,
// which is the best we can do here.
output = 0.0f;
_initialized = true;
}
setU(input);
return output;
}

17
src/modules/controllib/blocks.hpp
View File

@ -238,9 +238,25 @@ public:
BlockDerivative(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_u(0),
_initialized(false),
_lowPass(this, "LP")
{};
virtual ~BlockDerivative() {};
/**
* Update the state and get current derivative
*
* This call updates the state and gets the current
* derivative. As the derivative is only valid
* on the second call to update, it will return
* no change (0) on the first. To get a closer
* estimate of the derivative on the first call,
* call setU() one time step before using the
* return value of update().
*
* @param input the variable to calculate the derivative of
* @return the current derivative
*/
float update(float input);
// accessors
void setU(float u) {_u = u;}
@ -249,6 +265,7 @@ public:
protected:
// attributes
float _u; /**< previous input */
bool _initialized;
BlockLowPass _lowPass; /**< low pass filter */
};

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

@ -199,19 +199,6 @@ private:
float l1_period;
float l1_damping;
float time_const;
float min_sink_rate;
float max_sink_rate;
float max_climb_rate;
float throttle_damp;
float integrator_gain;
float vertical_accel_limit;
float height_comp_filter_omega;
float speed_comp_filter_omega;
float roll_throttle_compensation;
float speed_weight;
float pitch_damping;
float airspeed_min;
float airspeed_trim;
float airspeed_max;
@ -225,9 +212,6 @@ private:
float throttle_land_max;
float heightrate_p;
float speedrate_p;
float land_slope_angle;
float land_H1_virt;
float land_flare_alt_relative;
@ -242,19 +226,6 @@ private:
param_t l1_period;
param_t l1_damping;
param_t time_const;
param_t min_sink_rate;
param_t max_sink_rate;
param_t max_climb_rate;
param_t throttle_damp;
param_t integrator_gain;
param_t vertical_accel_limit;
param_t height_comp_filter_omega;
param_t speed_comp_filter_omega;
param_t roll_throttle_compensation;
param_t speed_weight;
param_t pitch_damping;
param_t airspeed_min;
param_t airspeed_trim;
param_t airspeed_max;
@ -268,9 +239,6 @@ private:
param_t throttle_land_max;
param_t heightrate_p;
param_t speedrate_p;
param_t land_slope_angle;
param_t land_H1_virt;
param_t land_flare_alt_relative;
@ -470,21 +438,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
_parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX");
_parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX");
_parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP");
_parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN");
_parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC");
_parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA");
_parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA");
_parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR");
_parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT");
_parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP");
_parameter_handles.heightrate_p = param_find("FW_T_HRATE_P");
_parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
/* fetch initial parameter values */
parameters_update();
}
@ -535,22 +488,6 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
param_get(_parameter_handles.time_const, &(_parameters.time_const));
param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp));
param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain));
param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit));
param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega));
param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega));
param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation));
param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight));
param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));

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

@ -154,182 +154,6 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
*/
PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
/**
* Maximum climb rate
*
* This is the best climb rate that the aircraft can achieve with
* the throttle set to THR_MAX and the airspeed set to the
* default value. For electric aircraft make sure this number can be
* achieved towards the end of flight when the battery voltage has reduced.
* The setting of this parameter can be checked by commanding a positive
* altitude change of 100m in loiter, RTL or guided mode. If the throttle
* required to climb is close to THR_MAX and the aircraft is maintaining
* airspeed, then this parameter is set correctly. If the airspeed starts
* to reduce, then the parameter is set to high, and if the throttle
* demand required to climb and maintain speed is noticeably less than
* FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
* FW_THR_MAX reduced.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
/**
* Minimum descent rate
*
* This is the sink rate of the aircraft with the throttle
* set to THR_MIN and flown at the same airspeed as used
* to measure FW_T_CLMB_MAX.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
/**
* Maximum descent rate
*
* This sets the maximum descent rate that the controller will use.
* If this value is too large, the aircraft can over-speed on descent.
* This should be set to a value that can be achieved without
* exceeding the lower pitch angle limit and without over-speeding
* the aircraft.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
/**
* TECS time constant
*
* This is the time constant of the TECS control algorithm (in seconds).
* Smaller values make it faster to respond, larger values make it slower
* to respond.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
/**
* Throttle damping factor
*
* This is the damping gain for the throttle demand loop.
* Increase to add damping to correct for oscillations in speed and height.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
/**
* Integrator gain
*
* This is the integrator gain on the control loop.
* Increasing this gain increases the speed at which speed
* and height offsets are trimmed out, but reduces damping and
* increases overshoot.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
/**
* Maximum vertical acceleration
*
* This is the maximum vertical acceleration (in metres/second square)
* either up or down that the controller will use to correct speed
* or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
* allows for reasonably aggressive pitch changes if required to recover
* from under-speed conditions.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
/**
* Complementary filter "omega" parameter for height
*
* This is the cross-over frequency (in radians/second) of the complementary
* filter used to fuse vertical acceleration and barometric height to obtain
* an estimate of height rate and height. Increasing this frequency weights
* the solution more towards use of the barometer, whilst reducing it weights
* the solution more towards use of the accelerometer data.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
/**
* Complementary filter "omega" parameter for speed
*
* This is the cross-over frequency (in radians/second) of the complementary
* filter used to fuse longitudinal acceleration and airspeed to obtain an
* improved airspeed estimate. Increasing this frequency weights the solution
* more towards use of the arispeed sensor, whilst reducing it weights the
* solution more towards use of the accelerometer data.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
/**
* Roll -> Throttle feedforward
*
* Increasing this gain turn increases the amount of throttle that will
* be used to compensate for the additional drag created by turning.
* Ideally this should be set to approximately 10 x the extra sink rate
* in m/s created by a 45 degree bank turn. Increase this gain if
* the aircraft initially loses energy in turns and reduce if the
* aircraft initially gains energy in turns. Efficient high aspect-ratio
* aircraft (eg powered sailplanes) can use a lower value, whereas
* inefficient low aspect-ratio models (eg delta wings) can use a higher value.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
/**
* Speed <--> Altitude priority
*
* This parameter adjusts the amount of weighting that the pitch control
* applies to speed vs height errors. Setting it to 0.0 will cause the
* pitch control to control height and ignore speed errors. This will
* normally improve height accuracy but give larger airspeed errors.
* Setting it to 2.0 will cause the pitch control loop to control speed
* and ignore height errors. This will normally reduce airspeed errors,
* but give larger height errors. The default value of 1.0 allows the pitch
* control to simultaneously control height and speed.
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
* adjust its pitch angle to maintain airspeed, ignoring changes in height).
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
/**
* Pitch damping factor
*
* This is the damping gain for the pitch demand loop. Increase to add
* damping to correct for oscillations in height. The default value of 0.0
* will work well provided the pitch to servo controller has been tuned
* properly.
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
/**
* Height rate P factor
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
/**
* Speed rate P factor
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
/**
* Landing slope angle
*

10
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
View File

@ -59,6 +59,7 @@ mTecs::mTecs() :
_controlAltitude(this, "FPA", true),
_controlAirSpeed(this, "ACC"),
_flightPathAngleLowpass(this, "FPA_LP"),
_altitudeLowpass(this, "ALT_LP"),
_airspeedLowpass(this, "A_LP"),
_airspeedDerivative(this, "AD"),
_throttleSp(0.0f),
@ -93,18 +94,23 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
/* time measurement */
updateTimeMeasurement();
/* Filter altitude */
float altitudeFiltered = _altitudeLowpass.update(altitude);
/* calculate flight path angle setpoint from altitude setpoint */
float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude);
float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered);
/* Debug output */
if (_counter % 10 == 0) {
debug("***");
debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp);
debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
}
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
_status.altitude = altitude;
_status.altitudeFiltered = altitudeFiltered;
/* use flightpath angle setpoint for total energy control */

1
src/modules/fw_pos_control_l1/mtecs/mTecs.h
View File

@ -115,6 +115,7 @@ protected:
/* Other calculation Blocks */
control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
control::BlockLowPass _altitudeLowpass; /**< low pass filter for altitude */
control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */

7
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
View File

@ -174,6 +174,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
*/
PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
/**
* Lowpass (cutoff freq.) for altitude
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ALT_LP, 1.0f);
/**
* Lowpass (cutoff freq.) for the flight path angle
*

9
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -135,12 +135,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
}
}
if (home_alt > missionitem.altitude) {
/* calculate the global waypoint altitude */
float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
if (home_alt > wp_alt) {
if (throw_error) {
mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i);
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i);
return false;
} else {
mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i);
mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i);
return true;
}
}

2
src/modules/navigator/navigator_main.cpp
View File

@ -458,7 +458,7 @@ void
Navigator::publish_position_setpoint_triplet()
{
/* update navigation state */
/* TODO: set nav_state */
_pos_sp_triplet.nav_state = _vstatus.nav_state;
/* lazily publish the position setpoint triplet only once available */
if (_pos_sp_triplet_pub > 0) {

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

@ -1432,17 +1432,20 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GLOBAL POSITION SETPOINT --- */
if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.nav_state = 0; /* TODO: Fix this */
log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
log_msg.body.log_GPSP.type = buf.triplet.current.type;
log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
LOGBUFFER_WRITE_AND_COUNT(GPSP);
if (buf.triplet.current.valid) {
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
log_msg.body.log_GPSP.type = buf.triplet.current.type;
log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
LOGBUFFER_WRITE_AND_COUNT(GPSP);
}
}
/* --- VICON POSITION --- */
@ -1595,6 +1598,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.msg_type = LOG_TECS_MSG;
log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered;
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;

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

@ -334,6 +334,7 @@ struct log_GS1B_s {
struct log_TECS_s {
float altitudeSp;
float altitude;
float altitudeFiltered;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
@ -454,7 +455,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(TECS, "fffffffffffffffB", "ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
/* system-level messages, ID >= 0x80 */

2
src/modules/uORB/topics/position_setpoint_triplet.h
View File

@ -95,6 +95,8 @@ struct position_setpoint_triplet_s
struct position_setpoint_s previous;
struct position_setpoint_s current;
struct position_setpoint_s next;
unsigned nav_state; /**< report the navigation state */
};
/**

1
src/modules/uORB/topics/tecs_status.h
View File

@ -66,6 +66,7 @@ struct tecs_status_s {
float altitudeSp;
float altitude;
float altitudeFiltered;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;