Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 17:34:01 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Soaring: remove SpdHgt and use TECS direct

Browse Source
This commit is contained in:
Iampete1 2021-11-12 17:55:01 +00:00 committed by Andrew Tridgell
parent 1c195d01b8
commit 7bf1fe1277
3 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -142,8 +142,8 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
AP_GROUPEND AP_GROUPEND
}; };
SoaringController::SoaringController(AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms) : SoaringController::SoaringController(AP_TECS &tecs, const AP_Vehicle::FixedWing &parms) :
_spdHgt(spdHgt), _tecs(tecs),
_vario(parms), _vario(parms),
_aparm(parms), _aparm(parms),
_throttle_suppressed(true) _throttle_suppressed(true)
@ -169,7 +169,7 @@ bool SoaringController::suppress_throttle()
set_throttle_suppressed(true); set_throttle_suppressed(true);
// Zero the pitch integrator - the nose is currently raised to climb, we need to go back to glide. // Zero the pitch integrator - the nose is currently raised to climb, we need to go back to glide.
_spdHgt.reset_pitch_I(); _tecs.reset_pitch_I();
_cruise_start_time_us = AP_HAL::micros64(); _cruise_start_time_us = AP_HAL::micros64();
@ -411,7 +411,7 @@ void SoaringController::set_throttle_suppressed(bool suppressed)
_throttle_suppressed = suppressed; _throttle_suppressed = suppressed;
// Let the TECS know. // Let the TECS know.
_spdHgt.set_gliding_requested_flag(suppressed); _tecs.set_gliding_requested_flag(suppressed);
} }
bool SoaringController::check_drift(Vector2f prev_wp, Vector2f next_wp) bool SoaringController::check_drift(Vector2f prev_wp, Vector2f next_wp)
@ -463,7 +463,7 @@ bool SoaringController::check_drift(Vector2f prev_wp, Vector2f next_wp)
float SoaringController::get_thermalling_radius() const float SoaringController::get_thermalling_radius() const
{ {
// Thermalling radius is controlled by parameter SOAR_THML_BANK and true target airspeed. // Thermalling radius is controlled by parameter SOAR_THML_BANK and true target airspeed.
const float target_aspd = _spdHgt.get_target_airspeed() * AP::ahrs().get_EAS2TAS(); const float target_aspd = _tecs.get_target_airspeed() * AP::ahrs().get_EAS2TAS();
const float radius = (target_aspd*target_aspd) / (GRAVITY_MSS * tanf(thermal_bank*DEG_TO_RAD)); const float radius = (target_aspd*target_aspd) / (GRAVITY_MSS * tanf(thermal_bank*DEG_TO_RAD));
return radius; return radius;

6
libraries/AP_Soaring/AP_Soaring.h
View File

@ -14,7 +14,7 @@
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include "ExtendedKalmanFilter.h" #include "ExtendedKalmanFilter.h"
#include "Variometer.h" #include "Variometer.h"
#include <AP_SpdHgtControl/AP_SpdHgtControl.h> #include <AP_TECS/AP_TECS.h>
#ifndef HAL_SOARING_ENABLED #ifndef HAL_SOARING_ENABLED
#define HAL_SOARING_ENABLED !HAL_MINIMIZE_FEATURES #define HAL_SOARING_ENABLED !HAL_MINIMIZE_FEATURES
@ -30,7 +30,7 @@
class SoaringController { class SoaringController {
ExtendedKalmanFilter _ekf{}; ExtendedKalmanFilter _ekf{};
AP_SpdHgtControl &_spdHgt; AP_TECS &_tecs;
Variometer _vario; Variometer _vario;
const AP_Vehicle::FixedWing &_aparm; const AP_Vehicle::FixedWing &_aparm;
@ -77,7 +77,7 @@ protected:
AP_Float thermal_bank; AP_Float thermal_bank;
public: public:
SoaringController(AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms); SoaringController(AP_TECS &tecs, const AP_Vehicle::FixedWing &parms);
enum class LoiterStatus { enum class LoiterStatus {
DISABLED, DISABLED,

2
libraries/AP_Soaring/Variometer.h
View File

@ -7,8 +7,8 @@ Manages the estimation of aircraft total energy, drag and vertical air velocity.
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
#include <AP_Param/AP_Param.h> #include <AP_Param/AP_Param.h>
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
#include <Filter/AverageFilter.h> #include <Filter/AverageFilter.h>
#include <AP_Vehicle/AP_Vehicle.h>
class Variometer { class Variometer {