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

Merge branch 'release_v1.0.0' into stable

Browse Source
This commit is contained in:
Lorenz Meier
2015-07-04 17:27:12 +02:00
parent 47358a446f 4372701dab
commit 364d323ddf
12 changed files with 103 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/3033_wingwing
-5
View File
@@ -23,12 +23,7 @@ then
param set FW_LND_TLALT 5
param set FW_THR_LND_MAX 0
param set FW_PR_FF 0.35
param set FW_PR_I 0.005
param set FW_PR_IMAX 0.4
param set FW_PR_P 0.08
param set FW_RR_FF 0.6
param set FW_RR_I 0.005
param set FW_RR_IMAX 0.2
param set FW_RR_P 0.04
fi
makefiles/config_aerocore_default.mk
+4 -3
View File
@@ -48,11 +48,12 @@ MODULES += modules/navigator
MODULES += modules/mavlink
#
# Estimation modules (EKF/ SO3 / other filters)
# Estimation modules (EKF / other filters)
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_so3
# Too high RAM usage due to static allocations
#MODULES += modules/attitude_estimator_ekf
MODULES += modules/ekf_att_pos_estimator
MODULES += modules/attitude_estimator_q
MODULES += modules/position_estimator_inav
#
makefiles/config_px4fmu-v2_default.mk
+2 -1
View File
@@ -76,7 +76,8 @@ MODULES += modules/land_detector
#
# Estimation modules (EKF/ SO3 / other filters)
#
MODULES += modules/attitude_estimator_ekf
# Too high RAM usage due to static allocations
#MODULES += modules/attitude_estimator_ekf
MODULES += modules/attitude_estimator_q
MODULES += modules/ekf_att_pos_estimator
MODULES += modules/position_estimator_inav
src/modules/commander/commander.cpp
+23 -8
View File
@@ -1479,7 +1479,22 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
/* position changed */
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
vehicle_global_position_s gpos;
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &gpos);
/* copy to global struct if valid, with hysteresis */
// XXX consolidate this with local position handling and timeouts after release
// but we want a low-risk change now.
if (status.condition_global_position_valid) {
if (gpos.eph < eph_threshold * 2.5f) {
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
}
} else {
if (gpos.eph < eph_threshold) {
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
}
}
}
/* update local position estimate */
@@ -1492,17 +1507,16 @@ int commander_thread_main(int argc, char *argv[])
//update condition_global_position_valid
//Global positions are only published by the estimators if they are valid
if(hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
//We have had no good fix for POSITION_TIMEOUT amount of time
if(status.condition_global_position_valid) {
if (status.condition_global_position_valid) {
set_tune_override(TONE_GPS_WARNING_TUNE);
status_changed = true;
status.condition_global_position_valid = false;
}
}
else if(global_position.timestamp != 0) {
//Got good global position estimate
if(!status.condition_global_position_valid) {
} else if (global_position.timestamp != 0) {
// Got good global position estimate
if (!status.condition_global_position_valid) {
status_changed = true;
status.condition_global_position_valid = true;
}
@@ -2637,7 +2651,8 @@ set_control_mode()
control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position;
control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_position;
control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position;
break;
src/modules/commander/commander_helper.cpp
+6 -2
View File
@@ -382,8 +382,12 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float
counter++;
/* remaining charge estimate based on voltage and internal resistance (drop under load) */
float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized);
float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty));
float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_normalized);
/* the range from full to empty is the same for batteries under load and without load,
* since the voltage drop applies to both the full and empty state
*/
float voltage_range = (bat_v_full - bat_v_empty);
float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty_dynamic)) / (bat_n_cells * voltage_range);
if (bat_capacity > 0.0f) {
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+13 -3
View File
@@ -74,6 +74,8 @@ static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we si
static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds)
static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds)
static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds)
static constexpr float EPH_LARGE_VALUE = 1000.0f;
static constexpr float EPV_LARGE_VALUE = 1000.0f;
/**
* estimator app start / stop handling function
@@ -924,8 +926,16 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
_global_pos.yaw = _local_pos.yaw;
_global_pos.eph = _gps.eph;
_global_pos.epv = _gps.epv;
const float dtLastGoodGPS = static_cast<float>(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f;
if (_gps.timestamp_position == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) {
_global_pos.eph = EPH_LARGE_VALUE;
_global_pos.epv = EPV_LARGE_VALUE;
} else {
_global_pos.eph = _gps.eph;
_global_pos.epv = _gps.epv;
}
if (!isfinite(_global_pos.lat) ||
!isfinite(_global_pos.lon) ||
@@ -1424,7 +1434,7 @@ void AttitudePositionEstimatorEKF::pollData()
// If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update,
// then something is very wrong with the GPS (possibly a hardware failure or comlink error)
const float dtLastGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
const float dtLastGoodGPS = static_cast<float>(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f;
if (dtLastGoodGPS >= POS_RESET_THRESHOLD) {
src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
+13 -4
View File
@@ -38,7 +38,6 @@
*/
#include "estimator_utilities.h"
#include <algorithm>
// Define EKF_DEBUG here to enable the debug print calls
// if the macro is not set, these will be completely
@@ -72,6 +71,9 @@ ekf_debug(const char *fmt, ...)
void ekf_debug(const char *fmt, ...) { while(0){} }
#endif
/* we don't want to pull in the standard lib just to swap two floats */
void swap_var(float &d1, float &d2);
float Vector3f::length(void) const
{
return sqrt(x*x + y*y + z*z);
@@ -108,9 +110,9 @@ void Mat3f::identity() {
Mat3f Mat3f::transpose() const
{
Mat3f ret = *this;
std::swap(ret.x.y, ret.y.x);
std::swap(ret.x.z, ret.z.x);
std::swap(ret.y.z, ret.z.y);
swap_var(ret.x.y, ret.y.x);
swap_var(ret.x.z, ret.z.x);
swap_var(ret.y.z, ret.z.y);
return ret;
}
@@ -223,3 +225,10 @@ Vector3f operator/(const Vector3f &vec, const float scalar)
vecOut.z = vec.z / scalar;
return vecOut;
}
void swap_var(float &d1, float &d2)
{
float tmp = d1;
d1 = d2;
d2 = tmp;
}
src/modules/fw_att_control/fw_att_control_main.cpp
+13 -6
View File
@@ -803,6 +803,14 @@ FixedwingAttitudeControl::task_main()
//warnx("_actuators_airframe.control[1] = -1.0f;");
}
/* default flaps to center */
float flaps_control = 0.0f;
/* map flaps by default to manual if valid */
if (isfinite(_manual.flaps)) {
flaps_control = _manual.flaps;
}
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_attitude_enabled) {
@@ -922,7 +930,6 @@ FixedwingAttitudeControl::task_main()
/* allow manual control of rudder deflection */
yaw_manual = _manual.r;
throttle_sp = _manual.z;
_actuators.control[4] = _manual.flaps;
/*
* in manual mode no external source should / does emit attitude setpoints.
@@ -1085,13 +1092,13 @@ FixedwingAttitudeControl::task_main()
} else {
/* manual/direct control */
_actuators.control[0] = _manual.y + _parameters.trim_roll;
_actuators.control[1] = -_manual.x + _parameters.trim_pitch;
_actuators.control[2] = _manual.r + _parameters.trim_yaw;
_actuators.control[3] = _manual.z;
_actuators.control[4] = _manual.flaps;
_actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y + _parameters.trim_roll;
_actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x + _parameters.trim_pitch;
_actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r + _parameters.trim_yaw;
_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
}
_actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_control;
_actuators.control[5] = _manual.aux1;
_actuators.control[6] = _manual.aux2;
_actuators.control[7] = _manual.aux3;
src/modules/fw_att_control/fw_att_control_params.c
+6 -6
View File
@@ -64,7 +64,7 @@
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.4f);
/**
* Pitch rate proportional gain.
@@ -76,7 +76,7 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f);
PARAM_DEFINE_FLOAT(FW_PR_P, 0.08f);
/**
* Pitch rate integrator gain.
@@ -88,7 +88,7 @@ PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f);
* @max 50.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_PR_I, 0.005f);
/**
* Maximum positive / up pitch rate.
@@ -126,7 +126,7 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f);
/**
* Roll to Pitch feedforward gain.
@@ -161,7 +161,7 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
* @max 100.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f);
PARAM_DEFINE_FLOAT(FW_RR_I, 0.005f);
/**
* Roll Integrator Anti-Windup
@@ -247,7 +247,7 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
* @max 10.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f);
PARAM_DEFINE_FLOAT(FW_RR_FF, 0.5f);
/**
* Pitch rate feed forward
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+13 -4
View File
@@ -58,7 +58,7 @@
* @max 100.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f);
PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 20.0f);
/**
* L1 damping
@@ -80,7 +80,7 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
* @max 1.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f);
PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.6f);
/**
* Throttle max slew rate
@@ -123,10 +123,11 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
* The maximum roll the controller will output.
*
* @unit degrees
* @min 0.0
* @min 35.0
* @max 65.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f);
/**
* Throttle limit max
@@ -151,6 +152,8 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
* For aircraft with internal combustion engine this parameter should be set
* for desired idle rpm.
*
* @min 0.0
* @max 1.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
@@ -161,6 +164,8 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
* This throttle value will be set as throttle limit at FW_LND_TLALT,
* before arcraft will flare.
*
* @min 0.0
* @max 1.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
@@ -173,6 +178,8 @@ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
* distance to the desired altitude. Mostly used for takeoff waypoints / modes.
* Set to zero to disable climbout mode (not recommended).
*
* @min 0.0
* @max 150.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
@@ -193,6 +200,8 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
* FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
* FW_THR_MAX reduced.
*
* @min 2.0
* @max 10.0
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
src/modules/px4iofirmware/mixer.cpp
+8 -5
View File
@@ -287,15 +287,18 @@ mixer_tick(void)
} else if (mixer_servos_armed && should_always_enable_pwm) {
/* set the disarmed servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
up_pwm_servo_set(i, r_page_servo_disarmed[i]);
}
/* set S.BUS1 or S.BUS2 outputs */
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT)
sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
sbus1_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT);
}
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT)
sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
sbus2_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT);
}
}
}
src/platforms/ros/nodes/commander/commander.cpp
+2 -1
View File
@@ -135,7 +135,8 @@ void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboar
msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position;
msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_position;
msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_velocity ||
!msg_offboard_control_mode.ignore_position;
}
void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,