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 321f742fb5..f9b0b1cbca 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,9 +74,12 @@ static uint64_t IMUusec = 0; static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure -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 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; @@ -118,7 +121,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _task_running(false), _estimator_task(-1), -/* subscriptions */ + /* subscriptions */ _sensor_combined_sub(-1), _distance_sub(-1), _airspeed_sub(-1), @@ -163,7 +166,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _sensor_combined{}, - _pos_ref{}, + _pos_ref{}, _filter_ref_offset(0.0f), _baro_gps_offset(0.0f), @@ -177,7 +180,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), - /* states */ + /* states */ _gps_alt_filt(0.0f), _baro_alt_filt(0.0f), _covariancePredictionDt(0.0f), @@ -270,6 +273,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF() } } while (_estimator_task != -1); } + delete _terrain_estimator; delete _ekf; @@ -324,13 +328,13 @@ int AttitudePositionEstimatorEKF::parameters_update() _ekf->accelProcessNoise = _parameters.acc_pnoise; _ekf->airspeedMeasurementSigma = _parameters.eas_noise; _ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF - #if 0 +#if 0 // Initially disable loading until // convergence is flight-test proven _ekf->magBias.x = _mag_offset_x.get(); _ekf->magBias.y = _mag_offset_y.get(); _ekf->magBias.z = _mag_offset_z.get(); - #endif +#endif } return OK; @@ -453,18 +457,18 @@ int AttitudePositionEstimatorEKF::check_filter_state() if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { PX4_INFO("health: VEL:%s POS:%s HGT:%s OFFS:%s", - ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); + ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); } if (rep.timeout_flags) { PX4_INFO("timeout: %s%s%s%s", - ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), - ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), - ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), - ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); + ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), + ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), + ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), + ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); } } @@ -476,7 +480,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() // Copy diagonal elemnts of covariance matrix float covariances[28]; _ekf->get_covariance(covariances); - + for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; rep.covariances[i] = covariances[i]; @@ -645,6 +649,7 @@ void AttitudePositionEstimatorEKF::task_main() if (_gpsIsGood) { _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; } + // if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { // _last_debug_print = hrt_absolute_time(); // perf_print_counter(_perf_baro); @@ -733,13 +738,15 @@ void AttitudePositionEstimatorEKF::task_main() } void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp, - bool gps_valid, double lat, double lon, float gps_alt, float baro_alt) + bool gps_valid, double lat, double lon, float gps_alt, float baro_alt) { // Reference altitude if (PX4_ISFINITE(_ekf->states[9])) { _filter_ref_offset = _ekf->states[9]; + } else if (PX4_ISFINITE(-_ekf->hgtMea)) { _filter_ref_offset = -_ekf->hgtMea; + } else { _filter_ref_offset = -_baro.altitude; } @@ -795,11 +802,11 @@ void AttitudePositionEstimatorEKF::initializeGPS() #if 0 PX4_INFO("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, - (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); + (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); PX4_INFO("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, - (double)_filter_ref_offset); + (double)_filter_ref_offset); PX4_INFO("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, - (double)math::degrees(declination)); + (double)math::degrees(declination)); #endif _gps_initialized = true; @@ -904,6 +911,7 @@ void AttitudePositionEstimatorEKF::publishControlState() } else { _ctrl_state.airspeed_valid = false; } + /* Attitude Rates */ _ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; _ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; @@ -911,12 +919,11 @@ void AttitudePositionEstimatorEKF::publishControlState() /* Guard from bad data */ if (!PX4_ISFINITE(_ctrl_state.x_vel) || - !PX4_ISFINITE(_ctrl_state.y_vel) || - !PX4_ISFINITE(_ctrl_state.z_vel) || - !PX4_ISFINITE(_ctrl_state.x_pos) || - !PX4_ISFINITE(_ctrl_state.y_pos) || - !PX4_ISFINITE(_ctrl_state.z_pos)) - { + !PX4_ISFINITE(_ctrl_state.y_vel) || + !PX4_ISFINITE(_ctrl_state.z_vel) || + !PX4_ISFINITE(_ctrl_state.x_pos) || + !PX4_ISFINITE(_ctrl_state.y_pos) || + !PX4_ISFINITE(_ctrl_state.z_pos)) { // bad data, abort publication return; } @@ -956,12 +963,11 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.yaw = _att.yaw; if (!PX4_ISFINITE(_local_pos.x) || - !PX4_ISFINITE(_local_pos.y) || - !PX4_ISFINITE(_local_pos.z) || - !PX4_ISFINITE(_local_pos.vx) || - !PX4_ISFINITE(_local_pos.vy) || - !PX4_ISFINITE(_local_pos.vz)) - { + !PX4_ISFINITE(_local_pos.y) || + !PX4_ISFINITE(_local_pos.z) || + !PX4_ISFINITE(_local_pos.vx) || + !PX4_ISFINITE(_local_pos.vy) || + !PX4_ISFINITE(_local_pos.vz)) { // bad data, abort publication return; } @@ -987,6 +993,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_utc_usec = _gps.time_utc_usec; + } else { _global_pos.lat = 0.0; _global_pos.lon = 0.0; @@ -1007,6 +1014,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; + } else { _global_pos.vel_d = 0.0f; } @@ -1015,6 +1023,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (_terrain_estimator->is_valid()) { _global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground(); _global_pos.terrain_alt_valid = true; + } else { _global_pos.terrain_alt_valid = false; } @@ -1028,8 +1037,8 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (!_local_pos.xy_global || !_local_pos.v_xy_valid || - _gps.timestamp_position == 0 || - (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { + _gps.timestamp_position == 0 || + (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { _global_pos.eph = EPH_LARGE_VALUE; _global_pos.epv = EPV_LARGE_VALUE; @@ -1040,12 +1049,11 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() } if (!PX4_ISFINITE(_global_pos.lat) || - !PX4_ISFINITE(_global_pos.lon) || - !PX4_ISFINITE(_global_pos.alt) || - !PX4_ISFINITE(_global_pos.vel_n) || - !PX4_ISFINITE(_global_pos.vel_e) || - !PX4_ISFINITE(_global_pos.vel_d)) - { + !PX4_ISFINITE(_global_pos.lon) || + !PX4_ISFINITE(_global_pos.alt) || + !PX4_ISFINITE(_global_pos.vel_n) || + !PX4_ISFINITE(_global_pos.vel_e) || + !PX4_ISFINITE(_global_pos.vel_d)) { // bad data, abort publication return; } @@ -1102,6 +1110,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const if (_prediction_steps < MAX_PREDICITION_STEPS && ((hrt_absolute_time() - _prediction_last) < 20 * 1000)) { _prediction_steps++; return; + } else { _prediction_steps = 0; _prediction_last = hrt_absolute_time(); @@ -1221,11 +1230,11 @@ int AttitudePositionEstimatorEKF::start() /* start the task */ _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 4600, - (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 4600, + (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, + nullptr); if (_estimator_task < 0) { warn("task start failed"); @@ -1242,7 +1251,7 @@ void AttitudePositionEstimatorEKF::print_status() math::Vector<3> euler = R.to_euler(); PX4_INFO("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", - (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); + (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); // State vector: // 0-3: quaternions (q0, q1, q2, q3) @@ -1258,51 +1267,51 @@ void AttitudePositionEstimatorEKF::print_status() PX4_INFO("alt RAW: baro alt: %8.4f GPS alt: %8.4f", (double)_baro.altitude, (double)_ekf->gpsHgt); PX4_INFO("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)", (double)(_local_pos.z), (double)_global_pos.alt); PX4_INFO("filter ref offset: %8.4f baro GPS offset: %8.4f", (double)_filter_ref_offset, - (double)_baro_gps_offset); + (double)_baro_gps_offset); PX4_INFO("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, - (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); + (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, - (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, - (double)_ekf->correctedDelAng.z); + (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, + (double)_ekf->correctedDelAng.z); PX4_INFO("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1], - (double)_ekf->states[2], (double)_ekf->states[3]); + (double)_ekf->states[2], (double)_ekf->states[3]); PX4_INFO("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5], - (double)_ekf->states[6]); + (double)_ekf->states[6]); PX4_INFO("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[7], (double)_ekf->states[8], - (double)_ekf->states[9]); + (double)_ekf->states[9]); PX4_INFO("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[10], (double)_ekf->states[11], - (double)_ekf->states[12]); + (double)_ekf->states[12]); if (EKF_STATE_ESTIMATES == 23) { PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]); PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]); PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17], - (double)_ekf->states[18]); + (double)_ekf->states[18]); PX4_INFO("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20], - (double)_ekf->states[21]); + (double)_ekf->states[21]); PX4_INFO("states (terrain) [22]: %8.4f", (double)_ekf->states[22]); } else { PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]); PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]); PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17], - (double)_ekf->states[18]); + (double)_ekf->states[18]); PX4_INFO("states (mag bias) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20], - (double)_ekf->states[21]); + (double)_ekf->states[21]); } PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s", - (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", - (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", - (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", - (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", - (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", + (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", + (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", + (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); PX4_INFO("gyro status:"); _voter_gyro.print(); @@ -1333,6 +1342,7 @@ void AttitudePositionEstimatorEKF::pollData() if (PX4_ISFINITE(_ekf->dtIMUfilt) && _ekf->dtIMUfilt < 0.5f && _ekf->dtIMUfilt > 0.0001f) { deltaT = _ekf->dtIMUfilt; + } else { deltaT = 0.01f; } @@ -1345,16 +1355,17 @@ void AttitudePositionEstimatorEKF::pollData() for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) { _voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3], - _sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]); + _sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]); _voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3], - _sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]); + _sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]); _voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3], - _sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]); + _sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]); } // Get best measurement values hrt_abstime curr_time = hrt_absolute_time(); (void)_voter_gyro.get_best(curr_time, &_gyro_main); + if (_gyro_main >= 0) { // Use pre-integrated values if possible @@ -1362,12 +1373,15 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0]; _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1]; _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2]; + } else { float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f; + if (PX4_ISFINITE(dt_gyro) && (dt_gyro < 0.5f) && (dt_gyro > 0.00001f)) { deltaT = dt_gyro; _ekf->dtIMU = deltaT; } + _ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU; _ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU; _ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU; @@ -1380,6 +1394,7 @@ void AttitudePositionEstimatorEKF::pollData() } (void)_voter_accel.get_best(curr_time, &_accel_main); + if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) { // Use pre-integrated values if possible @@ -1387,6 +1402,7 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0]; _ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1]; _ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2]; + } else { _ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]) * _ekf->dtIMU; _ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]) * _ekf->dtIMU; @@ -1400,9 +1416,10 @@ void AttitudePositionEstimatorEKF::pollData() } (void)_voter_mag.get_best(curr_time, &_mag_main); + if (_mag_main >= 0) { Vector3f mag(_sensor_combined.magnetometer_ga[_mag_main * 3 + 0], _sensor_combined.magnetometer_ga[_mag_main * 3 + 1], - _sensor_combined.magnetometer_ga[_mag_main * 3 + 2]); + _sensor_combined.magnetometer_ga[_mag_main * 3 + 2]); /* fail over to the 2nd mag if we know the first is down */ if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) { @@ -1416,26 +1433,28 @@ void AttitudePositionEstimatorEKF::pollData() } if (!_failsafe && (_voter_gyro.failover_count() > 0 || - _voter_accel.failover_count() > 0 || - _voter_mag.failover_count() > 0)) { + _voter_accel.failover_count() > 0 || + _voter_mag.failover_count() > 0)) { _failsafe = true; mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); } if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || - _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || - _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { + _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { if (_vibration_warning_timestamp == 0) { _vibration_warning_timestamp = curr_time; + } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) { _vibration_warning = true; mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d", - (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), - (int)(100 * _voter_accel.get_vibration_factor(curr_time)), - (int)(100 * _voter_mag.get_vibration_factor(curr_time))); + (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), + (int)(100 * _voter_accel.get_vibration_factor(curr_time)), + (int)(100 * _voter_mag.get_vibration_factor(curr_time))); } + } else { _vibration_warning_timestamp = 0; } @@ -1444,7 +1463,7 @@ void AttitudePositionEstimatorEKF::pollData() // XXX this is for assessing the filter performance // leave this in as long as larger improvements are still being made. - #if 0 +#if 0 float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f; float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f; @@ -1455,32 +1474,32 @@ void AttitudePositionEstimatorEKF::pollData() if (hrt_elapsed_time(&lastprint) > 1000000 || _sensor_combined.gyro_rad_s[0] > 4.0f) { PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u", - (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, - dtoverflow5, dtoverflow10); + (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, + dtoverflow5, dtoverflow10); PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", - (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z, - (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z); + (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z, + (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z); PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", - (double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]), - (double)(_sensor_combined.accelerometer_integral_m_s[0]), - (double)(_sensor_combined.accelerometer_integral_m_s[1]), - (double)(_sensor_combined.accelerometer_integral_m_s[2])); + (double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]), + (double)(_sensor_combined.accelerometer_integral_m_s[0]), + (double)(_sensor_combined.accelerometer_integral_m_s[1]), + (double)(_sensor_combined.accelerometer_integral_m_s[2])); PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", - (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT)); + (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT)); PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f", - (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed); + (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed); PX4_WARN("DRV rate: %8.4f, %8.4f, %8.4f", - (double)_sensor_combined.gyro_rad_s[0], - (double)_sensor_combined.gyro_rad_s[1], - (double)_sensor_combined.gyro_rad_s[2]); + (double)_sensor_combined.gyro_rad_s[0], + (double)_sensor_combined.gyro_rad_s[1], + (double)_sensor_combined.gyro_rad_s[2]); lastprint = hrt_absolute_time(); } @@ -1492,7 +1511,8 @@ void AttitudePositionEstimatorEKF::pollData() if (deltaT > 0.01f) { dtoverflow10++; } - #endif + +#endif _data_good = true; @@ -1652,6 +1672,7 @@ void AttitudePositionEstimatorEKF::pollData() } baro_last = _baro.timestamp; + if (!_baro_init) { _baro_init = true; _baro_alt_filt = _baro.altitude; @@ -1674,8 +1695,9 @@ void AttitudePositionEstimatorEKF::pollData() if (_newRangeData) { orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance); + if ((_distance.current_distance > _distance.min_distance) - && (_distance.current_distance < _distance.max_distance)) { + && (_distance.current_distance < _distance.max_distance)) { _ekf->rngMea = _distance.current_distance; _distance_last_valid = _distance.timestamp; 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 0ea5a20f8c..b5e49d67c8 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include #include #include @@ -347,7 +347,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _task_running(false), _control_task(-1), -/* subscriptions */ + /* subscriptions */ _ctrl_state_sub(-1), _accel_sub(-1), _vcontrol_mode_sub(-1), @@ -356,7 +356,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _global_pos_sub(-1), _vehicle_status_sub(-1), -/* publications */ + /* publications */ _rate_sp_pub(nullptr), _attitude_sp_pub(nullptr), _actuators_0_pub(nullptr), @@ -366,11 +366,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _actuators_id(0), _attitude_setpoint_id(0), -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), -/* states */ + /* states */ _setpoint_valid(false), _debug(false), _flaps_cmd_last(0), @@ -635,12 +635,14 @@ FixedwingAttitudeControl::vehicle_status_poll() if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (!_rates_sp_id) { if (_vehicle_status.is_vtol) { _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_fw); _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); + } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); @@ -727,8 +729,9 @@ FixedwingAttitudeControl::task_main() last_run = hrt_absolute_time(); /* guard against too large deltaT's */ - if (deltaT > 1.0f) + if (deltaT > 1.0f) { deltaT = 0.01f; + } /* load local copies */ orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); @@ -821,6 +824,7 @@ FixedwingAttitudeControl::task_main() if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[7] = 1.0f; //warnx("_actuators_airframe.control[1] = 1.0f;"); + } else { _actuators_airframe.control[7] = 0.0f; //warnx("_actuators_airframe.control[1] = -1.0f;"); @@ -838,13 +842,15 @@ FixedwingAttitudeControl::task_main() /* map flaps by default to manual if valid */ if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) { - flaps_control = 0.5f * (_manual.flaps + 1.0f ) * _parameters.flaps_scale; + flaps_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale; + } else if (_vcontrol_mode.flag_control_auto_enabled) { flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f; } // move the actual control value continuous with time static hrt_abstime t_flaps_changed = 0; + if (fabsf(flaps_control - _flaps_cmd_last) > 0.01f) { t_flaps_changed = hrt_absolute_time(); delta_flaps = flaps_control - _flaps_cmd_last; @@ -865,12 +871,14 @@ FixedwingAttitudeControl::task_main() /* map flaperons by default to manual if valid */ if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) { flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale; + } else if (_vcontrol_mode.flag_control_auto_enabled) { flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f; } // move the actual control value continuous with time static hrt_abstime t_flaperons_changed = 0; + if (fabsf(flaperon - _flaperons_cmd_last) > 0.01f) { t_flaperons_changed = hrt_absolute_time(); delta_flaperon = flaperon - _flaperons_cmd_last; @@ -880,7 +888,8 @@ FixedwingAttitudeControl::task_main() static float flaperon_applied = 0.0f; if (fabsf(flaperon_applied - flaperon) > 0.01f) { - flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) * (delta_flaperon) / 1000000; + flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) * + (delta_flaperon) / 1000000; } /* decide if in stabilized or full manual control */ @@ -908,13 +917,14 @@ FixedwingAttitudeControl::task_main() * * Forcing the scaling to this value allows reasonable handheld tests. */ - float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); + float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : + airspeed); /* Use min airspeed to calculate ground speed scaling region. * Don't scale below gspd_scaling_trim */ float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + - _global_pos.vel_e * _global_pos.vel_e); + _global_pos.vel_e * _global_pos.vel_e); float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f); float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); @@ -930,7 +940,7 @@ FixedwingAttitudeControl::task_main() * for sure not be set from the remote control values) */ if (_vcontrol_mode.flag_control_auto_enabled || - !_vcontrol_mode.flag_control_manual_enabled) { + !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; @@ -941,13 +951,16 @@ FixedwingAttitudeControl::task_main() if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } + if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } + if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); } + } else if (_vcontrol_mode.flag_control_velocity_enabled) { /* the pilot does not want to change direction, @@ -955,9 +968,10 @@ FixedwingAttitudeControl::task_main() */ if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) { roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; + } else { roll_sp = (_manual.y * _parameters.man_roll_max) - + _parameters.rollsp_offset_rad; + + _parameters.rollsp_offset_rad; } pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; @@ -967,16 +981,18 @@ FixedwingAttitudeControl::task_main() if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } + if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } + if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_altitude_enabled) { - /* + /* * Velocity should be controlled and manual is enabled. */ roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad; @@ -987,13 +1003,16 @@ FixedwingAttitudeControl::task_main() if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); } + if (_att_sp.pitch_reset_integral) { _pitch_ctrl.reset_integrator(); } + if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); } + } else { /* * Scale down roll and pitch as the setpoints are radians @@ -1029,6 +1048,7 @@ FixedwingAttitudeControl::task_main() if (_attitude_sp_pub != nullptr) { /* publish the attitude setpoint */ orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp); + } else if (_attitude_setpoint_id) { /* advertise and publish */ _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp); @@ -1090,6 +1110,7 @@ FixedwingAttitudeControl::task_main() /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ float roll_u = _roll_ctrl.control_bodyrate(control_input); _actuators.control[0] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; + if (!PX4_ISFINITE(roll_u)) { _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); @@ -1101,26 +1122,29 @@ FixedwingAttitudeControl::task_main() float pitch_u = _pitch_ctrl.control_bodyrate(control_input); _actuators.control[1] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; + if (!PX4_ISFINITE(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," - " airspeed %.4f, airspeed_scaling %.4f," - " roll_sp %.4f, pitch_sp %.4f," - " _roll_ctrl.get_desired_rate() %.4f," - " _pitch_ctrl.get_desired_rate() %.4f" - " att_sp.roll_body %.4f", - (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), - (double)airspeed, (double)airspeed_scaling, - (double)roll_sp, (double)pitch_sp, - (double)_roll_ctrl.get_desired_rate(), - (double)_pitch_ctrl.get_desired_rate(), - (double)_att_sp.roll_body); + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", + (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), + (double)airspeed, (double)airspeed_scaling, + (double)roll_sp, (double)pitch_sp, + (double)_roll_ctrl.get_desired_rate(), + (double)_pitch_ctrl.get_desired_rate(), + (double)_att_sp.roll_body); } } float yaw_u = 0.0f; + if (_att_sp.fw_control_yaw == true) { yaw_u = _wheel_ctrl.control_bodyrate(control_input); } @@ -1128,14 +1152,17 @@ FixedwingAttitudeControl::task_main() else { yaw_u = _yaw_ctrl.control_bodyrate(control_input); } + _actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ _actuators.control[2] += yaw_manual; + if (!PX4_ISFINITE(yaw_u)) { _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } @@ -1144,16 +1171,19 @@ FixedwingAttitudeControl::task_main() /* throttle passed through if it is finite and if no engine failure was * detected */ _actuators.control[3] = (PX4_ISFINITE(throttle_sp) && - !(_vehicle_status.engine_failure || - _vehicle_status.engine_failure_cmd)) ? - throttle_sp : 0.0f; + !(_vehicle_status.engine_failure || + _vehicle_status.engine_failure_cmd)) ? + throttle_sp : 0.0f; + if (!PX4_ISFINITE(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } } + } else { perf_count(_nonfinite_input_perf); + if (_debug && loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } @@ -1172,6 +1202,7 @@ FixedwingAttitudeControl::task_main() if (_rate_sp_pub != nullptr) { /* publish the attitude rates setpoint */ orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); + } else if (_rates_sp_id) { /* advertise the attitude rates setpoint */ _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); @@ -1197,15 +1228,15 @@ FixedwingAttitudeControl::task_main() _actuators_airframe.timestamp_sample = _ctrl_state.timestamp; /* Only publish if any of the proper modes are enabled */ - if(_vcontrol_mode.flag_control_rates_enabled || - _vcontrol_mode.flag_control_attitude_enabled || - _vcontrol_mode.flag_control_manual_enabled) - { + if (_vcontrol_mode.flag_control_rates_enabled || + _vcontrol_mode.flag_control_attitude_enabled || + _vcontrol_mode.flag_control_manual_enabled) { /* publish the actuator controls */ if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); + } else if (_actuators_id) { - _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); + _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } if (_actuators_2_pub != nullptr) { @@ -1236,11 +1267,11 @@ FixedwingAttitudeControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("fw_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1300, - (px4_main_t)&FixedwingAttitudeControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1300, + (px4_main_t)&FixedwingAttitudeControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); @@ -1287,13 +1318,15 @@ int fw_att_control_main(int argc, char *argv[]) printf("."); fflush(stdout); } + printf("\n"); } + return 0; } if (!strcmp(argv[1], "stop")) { - if (att_control::g_control == nullptr){ + if (att_control::g_control == nullptr) { warnx("not running"); return 1; } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 752b1fd793..d7aebbe0a8 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -108,8 +108,10 @@ static int _control_task = -1; /**< task handle for sensor task */ #define TAKEOFF_IDLE 0.2f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) #define T_ALT_TIMEOUT 1 // time after which we abort landing if terrain estimate is not valid -static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes -static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode +static constexpr float THROTTLE_THRESH = + 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes +static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = + 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; /** @@ -400,7 +402,7 @@ private: * @param waypoint_next the waypoint in the heading direction */ void get_waypoint_heading_distance(float heading, float distance, - struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init); + struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init); /** * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available @@ -439,7 +441,8 @@ private: const struct position_setpoint_triplet_s &_pos_sp_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, + const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -466,13 +469,13 @@ private: * XXX need to clean up/remove this function once mtecs fully replaces TECS */ void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, - float pitch_min_rad, float pitch_max_rad, - float throttle_min, float throttle_max, float throttle_cruise, - bool climbout_mode, float climbout_pitch_min_rad, - float altitude, - const math::Vector<3> &ground_speed, - unsigned mode = tecs_status_s::TECS_MODE_NORMAL, - bool pitch_max_special = false); + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad, + float altitude, + const math::Vector<3> &ground_speed, + unsigned mode = tecs_status_s::TECS_MODE_NORMAL, + bool pitch_max_special = false); }; @@ -494,7 +497,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _task_should_exit(false), _task_running(false), -/* subscriptions */ + /* subscriptions */ _global_pos_sub(-1), _pos_sp_triplet_sub(-1), _ctrl_state_sub(-1), @@ -504,15 +507,15 @@ FixedwingPositionControl::FixedwingPositionControl() : _manual_control_sub(-1), _sensor_combined_sub(-1), -/* publications */ + /* publications */ _attitude_sp_pub(nullptr), _tecs_status_pub(nullptr), _nav_capabilities_pub(nullptr), -/* publication ID */ + /* publication ID */ _attitude_setpoint_id(0), -/* states */ + /* states */ _ctrl_state(), _att_sp(), _nav_capabilities(), @@ -523,7 +526,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _pos_sp_triplet(), _sensor_combined(), -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), _hold_alt(0.0f), @@ -590,7 +593,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); - _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); + _parameter_handles.land_use_terrain_estimate = param_find("FW_LND_USETER"); _parameter_handles.land_airspeed_scale = param_find("FW_AIRSPD_SCALE"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); @@ -691,7 +694,8 @@ FixedwingPositionControl::parameters_update() _parameters.land_thrust_lim_alt_relative = 0.66f * _parameters.land_flare_alt_relative; } - param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); + param_get(_parameter_handles.land_heading_hold_horizontal_distance, + &(_parameters.land_heading_hold_horizontal_distance)); param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); @@ -732,7 +736,8 @@ FixedwingPositionControl::parameters_update() } /* Update the landing slope */ - landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_alt_relative, _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(); @@ -772,10 +777,12 @@ FixedwingPositionControl::vehicle_status_poll() if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (!_attitude_setpoint_id) { if (_vehicle_status.is_vtol) { _attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint); + } else { _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); } @@ -827,7 +834,7 @@ FixedwingPositionControl::control_state_poll() _roll = euler_angles(0); _pitch = euler_angles(1); _yaw = euler_angles(2); - + /* update TECS state */ _tecs.enable_airspeed(_airspeed_valid); } @@ -904,7 +911,8 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, + const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) { if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { @@ -917,11 +925,15 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ float distance = 0.0f; float delta_altitude = 0.0f; + if (pos_sp_triplet.previous.valid) { - distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, + pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt; + } else { - distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, + pos_sp_triplet.current.lon); delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt; } @@ -950,13 +962,14 @@ void FixedwingPositionControl::navigation_capabilities_publish() if (_nav_capabilities_pub != nullptr) { orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + } else { _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); } } void FixedwingPositionControl::get_waypoint_heading_distance(float heading, float distance, - struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init) + struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init) { waypoint_prev.valid = true; waypoint_prev.alt = _hold_alt; @@ -965,6 +978,7 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa // on init set first waypoint to momentary position waypoint_prev.lat = _global_pos.lat - cos(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; waypoint_prev.lon = _global_pos.lon - sin(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; + } else { /* for previous waypoint use the one still in front of us but shift it such that it is @@ -980,7 +994,8 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa waypoint_next.alt = _hold_alt; } -float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) +float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, + const struct vehicle_global_position_s &global_pos) { if (!PX4_ISFINITE(global_pos.terrain_alt)) { return land_setpoint_alt; @@ -989,17 +1004,20 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ if (_parameters.land_use_terrain_estimate && global_pos.terrain_alt_valid) { - if(!land_useterrain) { + if (!land_useterrain) { mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate"); land_useterrain = true; } + return global_pos.terrain_alt; + } else { return land_setpoint_alt; } } -float FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt, const struct vehicle_global_position_s &global_pos) +float FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt, + const struct vehicle_global_position_s &global_pos) { if (PX4_ISFINITE(global_pos.terrain_alt) && global_pos.terrain_alt_valid) { return global_pos.terrain_alt; @@ -1048,33 +1066,39 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) float pitch = -(_manual.x - deadBand) / factor; _hold_alt += (_parameters.max_sink_rate * dt) * pitch; _was_in_deadband = false; + } else if (_manual.x < - deadBand) { /* pitching up */ float pitch = -(_manual.x + deadBand) / factor; _hold_alt += (_parameters.max_climb_rate * dt) * pitch; _was_in_deadband = false; climbout_mode = (pitch > MANUAL_THROTTLE_CLIMBOUT_THRESH); + } else if (!_was_in_deadband) { - /* store altitude at which manual.x was inside deadBand - * The aircraft should immediately try to fly at this altitude - * as this is what the pilot expects when he moves the stick to the center */ + /* store altitude at which manual.x was inside deadBand + * The aircraft should immediately try to fly at this altitude + * as this is what the pilot expects when he moves the stick to the center */ _hold_alt = _global_pos.alt; _althold_epv = _global_pos.epv; _was_in_deadband = true; } + if (_vehicle_status.is_vtol) { if (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode) { _hold_alt = _global_pos.alt; } } + return climbout_mode; } -bool FixedwingPositionControl::in_takeoff_situation() { +bool FixedwingPositionControl::in_takeoff_situation() +{ const hrt_abstime delta_takeoff = 10000000; const float throttle_threshold = 0.1f; - if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) { + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold + && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) { return true; } @@ -1087,6 +1111,7 @@ void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitc if (in_takeoff_situation()) { *hold_altitude = _takeoff_ground_alt + _parameters.climbout_diff; *pitch_limit_min = math::radians(10.0f); + } else { *pitch_limit_min = _parameters.pitch_limit_min; } @@ -1097,9 +1122,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi const struct position_setpoint_triplet_s &pos_sp_triplet) { float dt = 0.01; // Using non zero value to a avoid division by zero + if (_control_position_last_called > 0) { dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f; } + _control_position_last_called = hrt_absolute_time(); bool setpoint = true; @@ -1114,14 +1141,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* tell TECS to update its state, but let it know when it cannot actually control the plane */ bool in_air_alt_control = (!_vehicle_status.condition_landed && - (_control_mode.flag_control_auto_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_altitude_enabled)); + (_control_mode.flag_control_auto_enabled || + _control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_altitude_enabled)); /* update TECS filters */ if (!_mTecs.getEnabled()) { _tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb, - accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); + accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control); } math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; @@ -1139,13 +1166,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _time_went_in_air = hrt_absolute_time(); _takeoff_ground_alt = _global_pos.alt; } + /* reset flag when airplane landed */ if (_vehicle_status.condition_landed) { _was_in_air = false; } if (_control_mode.flag_control_auto_enabled && - pos_sp_triplet.current.valid) { + pos_sp_triplet.current.valid) { /* AUTONOMOUS FLIGHT */ /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ @@ -1154,10 +1182,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_ctrl_state.airspeed); + } else { _tecs.reset_state(); } } + _control_mode_current = FW_POSCTRL_MODE_AUTO; /* reset hold altitude */ @@ -1211,39 +1241,42 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.yaw_body = _l1_control.nav_bearing(); tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, - pos_sp_triplet.current.loiter_direction, ground_speed_2d); + pos_sp_triplet.current.loiter_direction, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); float alt_sp; + if (_nav_capabilities.abort_landing == true) { // if we entered loiter due to an aborted landing, demand // altitude setpoint well above landing waypoint alt_sp = _pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff; + } else { // use altitude given by wapoint alt_sp = _pos_sp_triplet.current.alt; } if (in_takeoff_situation() || - ((_global_pos.alt < _pos_sp_triplet.current.alt + _parameters.climbout_diff ) && _nav_capabilities.abort_landing == true)) { - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); + ((_global_pos.alt < _pos_sp_triplet.current.alt + _parameters.climbout_diff) + && _nav_capabilities.abort_landing == true)) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), + math::radians(15.0f)); } tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { @@ -1257,13 +1290,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); - float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(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)); /* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */ float wp_distance_save = wp_distance; + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) { wp_distance_save = 0.0f; } @@ -1275,7 +1310,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<2> curr_wp_shifted; double lat; double lon; - create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat,pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); + create_waypoint_from_line_and_dist(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon, pos_sp_triplet.previous.lat, + pos_sp_triplet.previous.lon, -1000.0f, &lat, &lon); curr_wp_shifted(0) = (float)lat; curr_wp_shifted(1) = (float)lon; @@ -1289,9 +1325,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!land_noreturn_horizontal) {//set target_bearing in first occurrence if (pos_sp_triplet.previous.valid) { target_bearing = bearing_lastwp_currwp; + } else { target_bearing = _yaw; } + mavlink_log_info(_mavlink_fd, "#Landing, heading hold"); } @@ -1327,53 +1365,60 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be * equal to _pos_sp_triplet.current.alt */ float terrain_alt; + if (_parameters.land_use_terrain_estimate) { if (_global_pos.terrain_alt_valid) { // all good, have valid terrain altitude terrain_alt = _global_pos.terrain_alt; _t_alt_prev_valid = terrain_alt; _time_last_t_alt = hrt_absolute_time(); + } else if (_time_last_t_alt == 0) { // we have started landing phase but don't have valid terrain // wait for some time, maybe we will soon get a valid estimate // until then just use the altitude of the landing waypoint if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) { terrain_alt = _pos_sp_triplet.current.alt; + } else { // still no valid terrain, abort landing terrain_alt = _pos_sp_triplet.current.alt; _nav_capabilities.abort_landing = true; } + } else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000) - || land_noreturn_vertical) { + || land_noreturn_vertical) { // use previous terrain estimate for some time and hope to recover // if we are already flaring (land_noreturn_vertical) then just // go with the old estimate terrain_alt = _t_alt_prev_valid; + } else { // terrain alt was not valid for long time, abort landing terrain_alt = _t_alt_prev_valid; _nav_capabilities.abort_landing = true; } + } else { // no terrain estimation, just use landing waypoint altitude terrain_alt = _pos_sp_triplet.current.alt; } - + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_altitude_rel = _pos_sp_triplet.previous.valid ? - _pos_sp_triplet.previous.alt - terrain_alt : 0.0f; + _pos_sp_triplet.previous.alt - terrain_alt : 0.0f; - float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); + float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, + bearing_lastwp_currwp, bearing_airplane_currwp); /* Check if we should start flaring with a vertical and a * horizontal limit (with some tolerance) * The horizontal limit is only applied when we are in front of the wp */ if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) && - (wp_distance_save < landingslope.flare_length() + 5.0f)) || - land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + (wp_distance_save < landingslope.flare_length() + 5.0f)) || + land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ // /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ @@ -1386,32 +1431,34 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.yaw_body = target_bearing; _att_sp.fw_control_yaw = true; - if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { + if (_global_pos.alt < terrain_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; mavlink_log_info(_mavlink_fd, "#Landing, limiting throttle"); } - } + } - float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); + float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, + bearing_airplane_currwp); /* avoid climbout */ - if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) - { + if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) { flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, - calculate_target_airspeed(airspeed_land), eas2tas, - math::radians(_parameters.land_flare_pitch_min_deg), - math::radians(_parameters.land_flare_pitch_max_deg), - 0.0f, throttle_max, throttle_land, - false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(_parameters.pitch_limit_min), - _global_pos.alt, ground_speed, - land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); + calculate_target_airspeed(airspeed_land), eas2tas, + math::radians(_parameters.land_flare_pitch_min_deg), + math::radians(_parameters.land_flare_pitch_max_deg), + 0.0f, throttle_max, throttle_land, + false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians( + _parameters.pitch_limit_min), + _global_pos.alt, ground_speed, + land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND); if (!land_noreturn_vertical) { // just started with the flaring phase @@ -1419,52 +1466,58 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi height_flare = _global_pos.alt - terrain_alt; mavlink_log_info(_mavlink_fd, "#Landing, flaring"); land_noreturn_vertical = true; + } else { if (_global_pos.vel_d > 0.1f) { _att_sp.pitch_body = math::radians(_parameters.land_flare_pitch_min_deg) * - math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f); + math::constrain((height_flare - (_global_pos.alt - terrain_alt)) / height_flare, 0.0f, 1.0f); + } else { _att_sp.pitch_body = _att_sp.pitch_body; } } flare_curve_alt_rel_last = flare_curve_alt_rel; + } else { - /* intersect glide slope: - * minimize speed to approach speed - * if current position is higher than the slope follow the glide slope (sink to the - * glide slope) - * also if the system captures the slope it should stay - * on the slope (bool land_onslope) - * if current position is below the slope continue at previous wp altitude - * until the intersection with slope - * */ + /* intersect glide slope: + * minimize speed to approach speed + * if current position is higher than the slope follow the glide slope (sink to the + * glide slope) + * also if the system captures the slope it should stay + * on the slope (bool land_onslope) + * if current position is below the slope continue at previous wp altitude + * until the intersection with slope + * */ float altitude_desired_rel; + if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) { /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; + if (!land_onslope) { mavlink_log_info(_mavlink_fd, "#Landing, on slope"); land_onslope = true; } + } else { /* continue horizontally */ altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : - _global_pos.alt - terrain_alt; + _global_pos.alt - terrain_alt; } tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, - calculate_target_airspeed(airspeed_approach), eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - _parameters.throttle_max, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed); + calculate_target_airspeed(airspeed_approach), eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); } } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { @@ -1503,23 +1556,23 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed( - _runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min), - eas2tas, - math::radians(_parameters.pitch_limit_min), - takeoff_pitch_max_rad, - _parameters.throttle_min, - _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? - _parameters.throttle_cruise, - _runway_takeoff.climbout(), - math::radians(_runway_takeoff.getMinPitch( - _pos_sp_triplet.current.pitch_min, - 10.0f, - _parameters.pitch_limit_min)), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_TAKEOFF, - takeoff_pitch_max_deg != _parameters.pitch_limit_max); + calculate_target_airspeed( + _runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, + _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? + _parameters.throttle_cruise, + _runway_takeoff.climbout(), + math::radians(_runway_takeoff.getMinPitch( + _pos_sp_triplet.current.pitch_min, + 10.0f, + _parameters.pitch_limit_min)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); // assign values _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); @@ -1537,10 +1590,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* Perform launch detection */ if (launchDetector.launchDetectionEnabled() && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { /* Inform user that launchdetection is running */ static hrt_abstime last_sent = 0; - if(hrt_absolute_time() - last_sent > 4e6) { + + if (hrt_absolute_time() - last_sent > 4e6) { mavlink_log_critical(_mavlink_fd, "#Launchdetection running"); last_sent = hrt_absolute_time(); } @@ -1550,6 +1604,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update our copy of the launch detection state */ launch_detection_state = launchDetector.getLaunchDetected(); + } else { /* no takeoff detection --> fly */ launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; @@ -1566,8 +1621,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use * full throttle, otherwise we use the preTakeOff Throttle */ float takeoff_throttle = launch_detection_state != - LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? - launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; + LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ? + launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max; /* select maximum pitch: the launchdetector may impose another limit for the pitch * depending on the state of the launch */ @@ -1579,38 +1634,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(1.3f * _parameters.airspeed_min), - eas2tas, - math::radians(_parameters.pitch_limit_min), - takeoff_pitch_max_rad, - _parameters.throttle_min, takeoff_throttle, - _parameters.throttle_cruise, - true, - math::max(math::radians(_pos_sp_triplet.current.pitch_min), - math::radians(10.0f)), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_TAKEOFF, - takeoff_pitch_max_deg != _parameters.pitch_limit_max); + calculate_target_airspeed(1.3f * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, takeoff_throttle, + _parameters.throttle_cruise, + true, + math::max(math::radians(_pos_sp_triplet.current.pitch_min), + math::radians(10.0f)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); + math::radians(15.0f)); } else { tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(_parameters.airspeed_trim), - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - takeoff_throttle, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed); + calculate_target_airspeed(_parameters.airspeed_trim), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + takeoff_throttle, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); } + } else { /* Tell the attitude controller to stop integrating while we are waiting * for the launch */ @@ -1621,7 +1677,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Set default roll and pitch setpoints during detection phase */ _att_sp.roll_body = 0.0f; _att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min), - math::radians(10.0f)); + math::radians(10.0f)); } } @@ -1643,7 +1699,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } else if (_control_mode.flag_control_velocity_enabled && - _control_mode.flag_control_altitude_enabled) { + _control_mode.flag_control_altitude_enabled) { /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed, heading is set to a distant waypoint */ @@ -1654,27 +1710,30 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _hdg_hold_enabled = false; // this makes sure the waypoints are reset below _yaw_lock_engaged = false; } + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_ctrl_state.airspeed); + } else { _tecs.reset_state(); } } + _control_mode_current = FW_POSCTRL_MODE_POSITION; /* Get demanded airspeed */ float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; /* update desired altitude based on user pitch stick input */ bool climbout_requested = update_desired_altitude(dt); - /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground * and set limit to pitch angle to prevent stearing into ground */ float pitch_limit_min; @@ -1683,23 +1742,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* throttle limiting */ throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < THROTTLE_THRESH) { throttle_max = 0.0f; } tecs_update_pitch_throttle(_hold_alt, - altctrl_airspeed, - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - throttle_max, - _parameters.throttle_cruise, - climbout_requested, - ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_NORMAL); + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + throttle_max, + _parameters.throttle_cruise, + climbout_requested, + ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_NORMAL); /* heading control */ @@ -1731,7 +1791,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* we have a valid heading hold position, are we too close? */ if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, - _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) { + _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) { get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); } @@ -1752,9 +1812,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (in_takeoff_situation()) { /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), - math::radians(15.0f)); + math::radians(15.0f)); } } + } else { _hdg_hold_enabled = false; _yaw_lock_engaged = false; @@ -1767,27 +1828,30 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; } + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_ctrl_state.airspeed); + } else { _tecs.reset_state(); } } + _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; /* Get demanded airspeed */ float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; /* update desired altitude based on user pitch stick input */ bool climbout_requested = update_desired_altitude(dt); - /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground * and set limit to pitch angle to prevent stearing into ground */ float pitch_limit_min; @@ -1795,23 +1859,25 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* throttle limiting */ throttle_max = _parameters.throttle_max; + if (fabsf(_manual.z) < THROTTLE_THRESH) { throttle_max = 0.0f; } tecs_update_pitch_throttle(_hold_alt, - altctrl_airspeed, - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - throttle_max, - _parameters.throttle_cruise, - climbout_requested, - ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_NORMAL); + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + throttle_max, + _parameters.throttle_cruise, + climbout_requested, + ((climbout_requested) ? math::radians(10.0f) : pitch_limit_min), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_NORMAL); + } else { _control_mode_current = FW_POSCTRL_MODE_OTHER; @@ -1843,34 +1909,34 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = 0.0f; } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto - pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && - !_runway_takeoff.runwayTakeoffEnabled()) { - /* making sure again that the correct thrust is used, - * without depending on library calls for safety reasons */ + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && + !_runway_takeoff.runwayTakeoffEnabled()) { + /* making sure again that the correct thrust is used, + * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - _runway_takeoff.runwayTakeoffEnabled()) { + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + _runway_takeoff.runwayTakeoffEnabled()) { _att_sp.thrust = _runway_takeoff.getThrottle( - math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : - _tecs.get_throttle_demand(), throttle_max)); + math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : + _tecs.get_throttle_demand(), throttle_max)); } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && - pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; } else { /* Copy thrust and pitch values from tecs */ if (_vehicle_status.condition_landed && - (_control_mode_current == FW_POSCTRL_MODE_POSITION || _control_mode_current == FW_POSCTRL_MODE_ALTITUDE)) - { + (_control_mode_current == FW_POSCTRL_MODE_POSITION || _control_mode_current == FW_POSCTRL_MODE_ALTITUDE)) { // when we are landed in these modes we want the motor to spin _att_sp.thrust = math::min(TAKEOFF_IDLE, throttle_max); + } else { _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : - _tecs.get_throttle_demand(), throttle_max); + _tecs.get_throttle_demand(), throttle_max); } @@ -1879,17 +1945,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && ( - (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - (launch_detection_state == LAUNCHDETECTION_RES_NONE || - _runway_takeoff.runwayTakeoffEnabled())) || - (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && - land_noreturn_vertical)) - )) { + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && + (launch_detection_state == LAUNCHDETECTION_RES_NONE || + _runway_takeoff.runwayTakeoffEnabled())) || + (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND && + land_noreturn_vertical)) + )) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } if (_control_mode.flag_control_position_enabled) { last_manual = false; + } else { last_manual = true; } @@ -2017,7 +2084,8 @@ FixedwingPositionControl::task_main() float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON + if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) + || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0)) { /* set new turn distance */ @@ -2028,6 +2096,7 @@ FixedwingPositionControl::task_main() } } + perf_end(_loop_perf); } @@ -2074,10 +2143,13 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ /* Using mtecs library: prepare arguments for mtecs call */ float flightPathAngle = 0.0f; float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { - flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + flightPathAngle = -asinf(ground_speed(2) / ground_speed_length); } + fwPosctrl::LimitOverride limitOverride; + if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Force the slow downwards spiral */ limitOverride.enablePitchMinOverride(-1.0f); @@ -2085,6 +2157,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ } else if (climbout_mode) { limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { limitOverride.disablePitchMinOverride(); } @@ -2092,12 +2165,15 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ if (pitch_max_special) { /* Use the maximum pitch from the argument */ limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad); + } else { /* use pitch max set by MT param */ limitOverride.disablePitchMaxOverride(); } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode, - limitOverride); + limitOverride); + } else { if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Force the slow downwards spiral */ @@ -2105,8 +2181,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ pitch_max_rad = M_DEG_TO_RAD_F * 5.0f; } -/* No underspeed protection in landing mode */ - _tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); + /* No underspeed protection in landing mode */ + _tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND + || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp, @@ -2123,18 +2200,21 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ t.timestamp = s.timestamp; switch (s.mode) { - case TECS::ECL_TECS_MODE_NORMAL: - t.mode = tecs_status_s::TECS_MODE_NORMAL; - break; - case TECS::ECL_TECS_MODE_UNDERSPEED: - t.mode = tecs_status_s::TECS_MODE_UNDERSPEED; - break; - case TECS::ECL_TECS_MODE_BAD_DESCENT: - t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; - break; - case TECS::ECL_TECS_MODE_CLIMBOUT: - t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; - break; + case TECS::ECL_TECS_MODE_NORMAL: + t.mode = tecs_status_s::TECS_MODE_NORMAL; + break; + + case TECS::ECL_TECS_MODE_UNDERSPEED: + t.mode = tecs_status_s::TECS_MODE_UNDERSPEED; + break; + + case TECS::ECL_TECS_MODE_BAD_DESCENT: + t.mode = tecs_status_s::TECS_MODE_BAD_DESCENT; + break; + + case TECS::ECL_TECS_MODE_CLIMBOUT: + t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; + break; } t.altitudeSp = s.altitude_sp; @@ -2153,12 +2233,13 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ t.totalEnergyRateError = s.total_energy_rate_error; t.energyDistributionError = s.energy_distribution_error; t.energyDistributionRateError = s.energy_distribution_rate_error; - + t.throttle_integ = s.throttle_integ; t.pitch_integ = s.pitch_integ; if (_tecs_status_pub != nullptr) { orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); + } else { _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t); } @@ -2172,11 +2253,11 @@ FixedwingPositionControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("fw_pos_control_l1", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1300, - (px4_main_t)&FixedwingPositionControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1300, + (px4_main_t)&FixedwingPositionControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); @@ -2211,13 +2292,14 @@ int fw_pos_control_l1_main(int argc, char *argv[]) printf("."); fflush(stdout); } + printf("\n"); return 0; } if (!strcmp(argv[1], "stop")) { - if (l1_control::g_control == nullptr){ + if (l1_control::g_control == nullptr) { warnx("not running"); return 1; }