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 'release_v1.0.0'

Browse Source
This commit is contained in:
Lorenz Meier 2015-07-04 10:47:18 +02:00
commit 95eaebb28d
7 changed files with 53 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
makefiles/nuttx/config_aerocore_default.mk
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
#

3
makefiles/nuttx/config_px4fmu-v2_default.mk
View File

@ -77,7 +77,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

31
src/modules/commander/commander.cpp
View File

@ -1485,7 +1485,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 */
@ -1498,17 +1513,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;
}
@ -2642,7 +2656,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;

16
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -78,6 +78,8 @@ static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we sig
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
@ -936,8 +938,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 (!PX4_ISFINITE(_global_pos.lat) ||
!PX4_ISFINITE(_global_pos.lon) ||
@ -1436,7 +1446,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) {

13
src/modules/px4iofirmware/mixer.cpp
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);
}
}
}

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

@ -218,7 +218,6 @@ struct log_ARSP_s {
/* --- FLOW - OPTICAL FLOW --- */
#define LOG_FLOW_MSG 15
struct log_FLOW_s {
uint64_t timestamp;
uint8_t sensor_id;
float pixel_flow_x_integral;
float pixel_flow_y_integral;
@ -522,7 +521,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "QBffffffLLHhB", "IntT,ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"),
LOG_FORMAT(FLOW, "BffffffLLHhB", "ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"),
LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),

3
src/platforms/ros/nodes/commander/commander.cpp
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,