diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 708c34491b..3c2312fc7d 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -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 diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index c906d54189..0ce01d499d 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -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 # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 7884b94cb0..0998ebf9f8 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -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 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5ac8e9ca8b..f6fa5e6813 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 362a707c03..68949eec1d 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -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 */ diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 877bff6585..60be85b2cd 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -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(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(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; + const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; if (dtLastGoodGPS >= POS_RESET_THRESHOLD) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 284a099023..527420ba0b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -38,7 +38,6 @@ */ #include "estimator_utilities.h" -#include // 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; +} diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index ccf12a0791..9cbbe80cc3 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -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; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index d9ccd8bac8..234ff0c3fb 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -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 diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index c00d822327..78d66afe1e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -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); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index b5d93daea7..0106fa1eb7 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -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); + } } } diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index abaa6fc60d..54086cfd4b 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -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,