Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'ArduPilot:master' into master

Browse Source
This commit is contained in:
ZeroOne-Aero 2024-12-19 16:21:59 +08:00 committed by GitHub
commit 9cea8fe66a
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
34 changed files with 796 additions and 193 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
Rover/GCS_Mavlink.cpp
View File

@ -185,6 +185,65 @@ void GCS_MAVLINK_Rover::send_rangefinder() const
distance,
voltage);
}
void GCS_MAVLINK_Rover::send_water_depth() const
{
if (!HAVE_PAYLOAD_SPACE(chan, WATER_DEPTH)) {
return;
}
// only send for boats:
if (!rover.is_boat()) {
return;
}
RangeFinder *rangefinder = RangeFinder::get_singleton();
if (rangefinder == nullptr) {
return;
}
// depth can only be measured by a downward-facing rangefinder:
if (!rangefinder->has_orientation(ROTATION_PITCH_270)) {
return;
}
// get position
const AP_AHRS &ahrs = AP::ahrs();
Location loc;
IGNORE_RETURN(ahrs.get_location(loc));
for (uint8_t i=0; i<rangefinder->num_sensors(); i++) {
const AP_RangeFinder_Backend *s = rangefinder->get_backend(i);
if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) {
continue;
}
// get temperature
float temp_C;
if (!s->get_temp(temp_C)) {
temp_C = 0.0f;
}
const bool sensor_healthy = (s->status() == RangeFinder::Status::Good);
mavlink_msg_water_depth_send(
chan,
AP_HAL::millis(), // time since system boot TODO: take time of measurement
i, // rangefinder instance
sensor_healthy, // sensor healthy
loc.lat, // latitude of vehicle
loc.lng, // longitude of vehicle
loc.alt * 0.01f, // altitude of vehicle (MSL)
ahrs.get_roll(), // roll in radians
ahrs.get_pitch(), // pitch in radians
ahrs.get_yaw(), // yaw in radians
s->distance(), // distance in meters
temp_C); // temperature in degC
}
}
#endif // AP_RANGEFINDER_ENABLED
/*
@ -400,6 +459,13 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
}
#endif
#if AP_RANGEFINDER_ENABLED
case MSG_WATER_DEPTH:
CHECK_PAYLOAD_SIZE(WATER_DEPTH);
send_water_depth();
break;
#endif // AP_RANGEFINDER_ENABLED
default:
return GCS_MAVLINK::try_send_message(id);
}
@ -589,6 +655,7 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
MSG_WIND,
#if AP_RANGEFINDER_ENABLED
MSG_RANGEFINDER,
MSG_WATER_DEPTH,
#endif
MSG_DISTANCE_SENSOR,
MSG_SYSTEM_TIME,

5
Rover/GCS_Mavlink.h
View File

@ -29,8 +29,6 @@ protected:
void send_position_target_global_int() override;
bool persist_streamrates() const override { return true; }
uint64_t capabilities() const override;
void send_nav_controller_output() const override;
@ -44,6 +42,9 @@ protected:
// Index starts at 1
uint8_t send_available_mode(uint8_t index) const override;
// send WATER_DEPTH - metres and temperature
void send_water_depth() const;
private:
void handle_message(const mavlink_message_t &msg) override;

4
Rover/Log.cpp
View File

@ -79,10 +79,6 @@ void Rover::Log_Write_Depth()
(double)(s->distance()),
temp_C);
}
#if AP_RANGEFINDER_ENABLED
// send water depth and temp to ground station
gcs().send_message(MSG_WATER_DEPTH);
#endif
}
#endif

20
Tools/autotest/arducopter.py
View File

@ -1374,7 +1374,10 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
# cut motor 1's to efficiency
self.progress("Cutting motor 1 to 65% efficiency")
self.set_parameter("SIM_ENGINE_MUL", 0.65)
self.set_parameters({
"SIM_ENGINE_MUL": 0.65,
"SIM_ENGINE_FAIL": 1 << 0, # motor 1
})
while self.get_sim_time_cached() < tstart + holdtime:
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
@ -3285,8 +3288,8 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.progress("Killing motor %u (%u%%)" %
(fail_servo+1, fail_mul))
self.set_parameters({
"SIM_ENGINE_FAIL": fail_servo,
"SIM_ENGINE_MUL": fail_mul,
"SIM_ENGINE_FAIL": 1 << fail_servo,
})
failed = True
@ -3345,10 +3348,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
raise NotAchievedException("Vehicle is descending")
self.progress("Fixing motors")
self.set_parameters({
"SIM_ENGINE_FAIL": 0,
"SIM_ENGINE_MUL": 1.0,
})
self.set_parameter("SIM_ENGINE_FAIL", 0)
self.do_RTL()
@ -4170,8 +4170,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.takeoff(40)
self.set_rc(9, 1500)
self.set_parameters({
"SIM_ENGINE_MUL": 0,
"SIM_ENGINE_FAIL": 1,
"SIM_ENGINE_FAIL": 1 << 1, # motor 2
})
self.wait_statustext('BANG! Parachute deployed', timeout=60)
self.set_rc(9, 1000)
@ -4184,8 +4183,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.takeoff(loiter_alt, mode='LOITER')
self.set_rc(9, 1100)
self.set_parameters({
"SIM_ENGINE_MUL": 0,
"SIM_ENGINE_FAIL": 1,
"SIM_ENGINE_FAIL": 1 << 1, # motor 2
})
tstart = self.get_sim_time()
while self.get_sim_time_cached() < tstart + 5:
@ -12024,8 +12022,8 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.context_push()
self.context_collect('STATUSTEXT')
self.set_parameters({
"SIM_ENGINE_FAIL": 1,
"SIM_ENGINE_MUL": 0.5,
"SIM_ENGINE_FAIL": 1 << 1, # motor 2
"FLIGHT_OPTIONS": 4,
})

2
Tools/autotest/default_params/vee-gull_005.param
View File

@ -620,7 +620,7 @@ SIM_BATT_VOLTAGE,12.6
SIM_DRIFT_SPEED,0.05
SIM_DRIFT_TIME,5
SIM_ENGINE_FAIL,0
SIM_ENGINE_MUL,1
SIM_ENGINE_MUL,0
SIM_FLOAT_EXCEPT,1
SIM_FLOW_DELAY,0
SIM_FLOW_ENABLE,0

10
Tools/autotest/quadplane.py
View File

@ -767,8 +767,7 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
"Q_WVANE_ENABLE": 1,
"Q_WVANE_GAIN": 1,
"STICK_MIXING": 0,
"Q_FWD_THR_USE": 2,
"SIM_ENGINE_FAIL": 2}) # we want to fail the forward thrust motor only
"Q_FWD_THR_USE": 2})
self.takeoff(10, mode="QLOITER")
self.set_rc(2, 1000)
@ -785,7 +784,7 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
self.set_rc(2, 1500)
self.delay_sim_time(5)
loc1 = self.mav.location()
self.set_parameter("SIM_ENGINE_MUL", 0) # simulate a complete loss of forward motor thrust
self.set_parameter("SIM_ENGINE_FAIL", 1 << 2) # simulate a complete loss of forward motor thrust
self.delay_sim_time(20)
self.change_mode('QLAND')
self.wait_disarmed(timeout=60)
@ -1475,7 +1474,10 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
(p, new_values[p], threshold))
self.progress("ensure we are not overtuned")
self.set_parameter('SIM_ENGINE_MUL', 0.9)
self.set_parameters({
'SIM_ENGINE_MUL': 0.9,
'SIM_ENGINE_FAIL': 1 << 0,
})
self.delay_sim_time(5)

9
Tools/autotest/vehicle_test_suite.py
View File

@ -1715,8 +1715,8 @@ class FRSkySPort(FRSky):
if not self.connect():
self.progress("Failed to connect")
return
self.check_poll()
self.do_sport_read()
self.check_poll()
def do_sport_read(self):
self.buffer += self.do_read()
@ -13738,8 +13738,11 @@ switch value'''
def FRSkySPort(self):
'''Test FrSky SPort mode'''
self.set_parameter("SERIAL5_PROTOCOL", 4) # serial5 is FRSky sport
self.set_parameter("RPM1_TYPE", 10) # enable SITL RPM sensor
self.set_parameters({
"SERIAL5_PROTOCOL": 4, # serial5 is FRSky sport
"RPM1_TYPE": 10, # enable SITL RPM sensor
"GPS1_TYPE": 100, # use simulated backend for consistent position
})
port = self.spare_network_port()
self.customise_SITL_commandline([
"--serial5=tcp:%u" % port # serial5 spews to localhost port

22
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -1208,6 +1208,28 @@ bool AC_PosControl::get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &v
accel_offset_NED.z = -_accel_offset_target.z * 0.01;
return true;
}
// get target velocity in m/s in NED frame
bool AC_PosControl::get_vel_target(Vector3f &vel_target_NED)
{
if (!is_active_xy() || !is_active_z()) {
return false;
}
vel_target_NED.xy() = _vel_target.xy() * 0.01;
vel_target_NED.z = -_vel_target.z * 0.01;
return true;
}
// get target acceleration in m/s/s in NED frame
bool AC_PosControl::get_accel_target(Vector3f &accel_target_NED)
{
if (!is_active_xy() || !is_active_z()) {
return false;
}
accel_target_NED.xy() = _accel_target.xy() * 0.01;
accel_target_NED.z = -_accel_target.z * 0.01;
return true;
}
#endif
/// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame

6
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -339,6 +339,12 @@ public:
// units are m, m/s and m/s/s in NED frame
bool set_posvelaccel_offset(const Vector3f &pos_offset_NED, const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED);
bool get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &vel_offset_NED, Vector3f &accel_offset_NED);
// get target velocity in m/s in NED frame
bool get_vel_target(Vector3f &vel_target_NED);
// get target acceleration in m/s/s in NED frame
bool get_accel_target(Vector3f &accel_target_NED);
#endif
/// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame

28
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -346,9 +346,9 @@ void AP_AHRS::update_state(void)
state.primary_core = _get_primary_core_index();
state.wind_estimate_ok = _wind_estimate(state.wind_estimate);
state.EAS2TAS = AP_AHRS_Backend::get_EAS2TAS();
state.airspeed_ok = _airspeed_estimate(state.airspeed, state.airspeed_estimate_type);
state.airspeed_true_ok = _airspeed_estimate_true(state.airspeed_true);
state.airspeed_vec_ok = _airspeed_vector_true(state.airspeed_vec);
state.airspeed_ok = _airspeed_EAS(state.airspeed, state.airspeed_estimate_type);
state.airspeed_true_ok = _airspeed_TAS(state.airspeed_true);
state.airspeed_vec_ok = _airspeed_TAS(state.airspeed_vec);
state.quat_ok = _get_quaternion(state.quat);
state.secondary_attitude_ok = _get_secondary_attitude(state.secondary_attitude);
state.secondary_quat_ok = _get_secondary_quaternion(state.secondary_quat);
@ -931,7 +931,7 @@ bool AP_AHRS::_should_use_airspeed_sensor(uint8_t airspeed_index) const
// return an airspeed estimate if available. return true
// if we have an estimate
bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airspeed_estimate_type) const
bool AP_AHRS::_airspeed_EAS(float &airspeed_ret, AirspeedEstimateType &airspeed_estimate_type) const
{
#if AP_AHRS_DCM_ENABLED || (AP_AIRSPEED_ENABLED && AP_GPS_ENABLED)
const uint8_t idx = get_active_airspeed_index();
@ -970,20 +970,20 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(idx, airspeed_ret);
return dcm.airspeed_EAS(idx, airspeed_ret);
#endif
#if AP_AHRS_SIM_ENABLED
case EKFType::SIM:
airspeed_estimate_type = AirspeedEstimateType::SIM;
return sim.airspeed_estimate(airspeed_ret);
return sim.airspeed_EAS(airspeed_ret);
#endif
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
#if AP_AHRS_DCM_ENABLED
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(idx, airspeed_ret);
return dcm.airspeed_EAS(idx, airspeed_ret);
#else
return false;
#endif
@ -999,7 +999,7 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs
case EKFType::EXTERNAL:
#if AP_AHRS_DCM_ENABLED
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(idx, airspeed_ret);
return dcm.airspeed_EAS(idx, airspeed_ret);
#else
return false;
#endif
@ -1027,18 +1027,18 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs
#if AP_AHRS_DCM_ENABLED
// fallback to DCM
airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC;
return dcm.airspeed_estimate(idx, airspeed_ret);
return dcm.airspeed_EAS(idx, airspeed_ret);
#endif
return false;
}
bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const
bool AP_AHRS::_airspeed_TAS(float &airspeed_ret) const
{
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
case EKFType::DCM:
return dcm.airspeed_estimate_true(airspeed_ret);
return dcm.airspeed_TAS(airspeed_ret);
#endif
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
@ -1064,7 +1064,7 @@ bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const
// return estimate of true airspeed vector in body frame in m/s
// returns false if estimate is unavailable
bool AP_AHRS::_airspeed_vector_true(Vector3f &vec) const
bool AP_AHRS::_airspeed_TAS(Vector3f &vec) const
{
switch (active_EKF_type()) {
#if AP_AHRS_DCM_ENABLED
@ -1126,14 +1126,14 @@ bool AP_AHRS::airspeed_health_data(float &innovation, float &innovationVariance,
return false;
}
// return a synthetic airspeed estimate (one derived from sensors
// return a synthetic EAS estimate (one derived from sensors
// other than an actual airspeed sensor), if available. return
// true if we have a synthetic airspeed. ret will not be modified
// on failure.
bool AP_AHRS::synthetic_airspeed(float &ret) const
{
#if AP_AHRS_DCM_ENABLED
return dcm.synthetic_airspeed(ret);
return dcm.synthetic_airspeed_EAS(ret);
#endif
return false;
}

10
libraries/AP_AHRS/AP_AHRS.h
View File

@ -152,7 +152,7 @@ public:
// get air density / sea level density - decreases as altitude climbs
float get_air_density_ratio(void) const;
// return an airspeed estimate if available. return true
// return an (equivalent) airspeed estimate if available. return true
// if we have an estimate
bool airspeed_estimate(float &airspeed_ret) const;
@ -195,7 +195,7 @@ public:
return AP_AHRS_Backend::airspeed_sensor_enabled(airspeed_index);
}
// return a synthetic airspeed estimate (one derived from sensors
// return a synthetic (equivalent) airspeed estimate (one derived from sensors
// other than an actual airspeed sensor), if available. return
// true if we have a synthetic airspeed. ret will not be modified
// on failure.
@ -873,7 +873,7 @@ private:
// return an airspeed estimate if available. return true
// if we have an estimate
bool _airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &status) const;
bool _airspeed_EAS(float &airspeed_ret, AirspeedEstimateType &status) const;
// return secondary attitude solution if available, as eulers in radians
bool _get_secondary_attitude(Vector3f &eulers) const;
@ -892,11 +892,11 @@ private:
// return a true airspeed estimate (navigation airspeed) if
// available. return true if we have an estimate
bool _airspeed_estimate_true(float &airspeed_ret) const;
bool _airspeed_TAS(float &airspeed_ret) const;
// return estimate of true airspeed vector in body frame in m/s
// returns false if estimate is unavailable
bool _airspeed_vector_true(Vector3f &vec) const;
bool _airspeed_TAS(Vector3f &vec) const;
// return the quaternion defining the rotation from NED to XYZ (body) axes
bool _get_quaternion(Quaternion &quat) const WARN_IF_UNUSED;

9
libraries/AP_AHRS/AP_AHRS_Backend.h
View File

@ -135,13 +135,13 @@ public:
// return an airspeed estimate if available. return true
// if we have an estimate
virtual bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED { return false; }
virtual bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const { return false; }
virtual bool airspeed_EAS(float &airspeed_ret) const WARN_IF_UNUSED { return false; }
virtual bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const { return false; }
// return a true airspeed estimate (navigation airspeed) if
// available. return true if we have an estimate
bool airspeed_estimate_true(float &airspeed_ret) const WARN_IF_UNUSED {
if (!airspeed_estimate(airspeed_ret)) {
bool airspeed_TAS(float &airspeed_ret) const WARN_IF_UNUSED {
if (!airspeed_EAS(airspeed_ret)) {
return false;
}
airspeed_ret *= get_EAS2TAS();
@ -156,6 +156,7 @@ public:
// get apparent to true airspeed ratio
static float get_EAS2TAS(void);
static float get_TAS2EAS(void) { return 1.0/get_EAS2TAS(); }
// return true if airspeed comes from an airspeed sensor, as
// opposed to an IMU estimate

32
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -724,16 +724,16 @@ AP_AHRS_DCM::drift_correction(float deltat)
return;
}
float airspeed = _last_airspeed;
float airspeed_TAS = _last_airspeed_TAS;
#if AP_AIRSPEED_ENABLED
if (airspeed_sensor_enabled()) {
airspeed = AP::airspeed()->get_airspeed();
airspeed_TAS = AP::airspeed()->get_airspeed() * get_EAS2TAS();
}
#endif
// use airspeed to estimate our ground velocity in
// earth frame by subtracting the wind
velocity = _dcm_matrix.colx() * airspeed;
velocity = _dcm_matrix.colx() * airspeed_TAS;
// add in wind estimate
velocity += _wind;
@ -762,7 +762,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// take positive component in X direction. This mimics a pitot
// tube
_last_airspeed = MAX(airspeed.x, 0);
_last_airspeed_TAS = MAX(airspeed.x, 0);
}
if (have_gps()) {
@ -1084,31 +1084,31 @@ bool AP_AHRS_DCM::get_location(Location &loc) const
return _have_position;
}
bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const
bool AP_AHRS_DCM::airspeed_EAS(float &airspeed_ret) const
{
#if AP_AIRSPEED_ENABLED
const auto *airspeed = AP::airspeed();
if (airspeed != nullptr) {
return airspeed_estimate(airspeed->get_primary(), airspeed_ret);
return airspeed_EAS(airspeed->get_primary(), airspeed_ret);
}
#endif
// airspeed_estimate will also make this nullptr check and act
// appropriately when we call it with a dummy sensor ID.
return airspeed_estimate(0, airspeed_ret);
return airspeed_EAS(0, airspeed_ret);
}
// return an airspeed estimate:
// return an (equivalent) airspeed estimate:
// - from a real sensor if available
// - otherwise from a GPS-derived wind-triangle estimate (if GPS available)
// - otherwise from a cached wind-triangle estimate value (but returning false)
bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const
bool AP_AHRS_DCM::airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const
{
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order:
// airspeed as filled-in by an enabled airspeed sensor
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
// Or if none of the above, fills-in using the previous airspeed estimate
// Return false: if we are using the previous airspeed estimate
if (!get_unconstrained_airspeed_estimate(airspeed_index, airspeed_ret)) {
if (!get_unconstrained_airspeed_EAS(airspeed_index, airspeed_ret)) {
return false;
}
@ -1127,12 +1127,12 @@ bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret)
return true;
}
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order:
// airspeed as filled-in by an enabled airspeed sensor
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
// Or if none of the above, fills-in using the previous airspeed estimate
// Return false: if we are using the previous airspeed estimate
bool AP_AHRS_DCM::get_unconstrained_airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const
bool AP_AHRS_DCM::get_unconstrained_airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const
{
#if AP_AIRSPEED_ENABLED
if (airspeed_sensor_enabled(airspeed_index)) {
@ -1143,13 +1143,13 @@ bool AP_AHRS_DCM::get_unconstrained_airspeed_estimate(uint8_t airspeed_index, fl
if (AP::ahrs().get_wind_estimation_enabled() && have_gps()) {
// estimated via GPS speed and wind
airspeed_ret = _last_airspeed;
airspeed_ret = _last_airspeed_TAS * get_TAS2EAS();
return true;
}
// Else give the last estimate, but return false.
// This is used by the dead-reckoning code
airspeed_ret = _last_airspeed;
airspeed_ret = _last_airspeed_TAS * get_TAS2EAS();
return false;
}
@ -1182,7 +1182,7 @@ Vector2f AP_AHRS_DCM::groundspeed_vector(void)
Vector2f gndVelADS;
Vector2f gndVelGPS;
float airspeed = 0;
const bool gotAirspeed = airspeed_estimate_true(airspeed);
const bool gotAirspeed = airspeed_TAS(airspeed);
const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D);
if (gotAirspeed) {
const Vector2f airspeed_vector{_cos_yaw * airspeed, _sin_yaw * airspeed};

16
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -82,18 +82,18 @@ public:
// return an airspeed estimate if available. return true
// if we have an estimate
bool airspeed_estimate(float &airspeed_ret) const override;
bool airspeed_EAS(float &airspeed_ret) const override;
// return an airspeed estimate if available. return true
// if we have an estimate from a specific sensor index
bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const override;
bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const override;
// return a synthetic airspeed estimate (one derived from sensors
// return a synthetic EAS estimate (one derived from sensors
// other than an actual airspeed sensor), if available. return
// true if we have a synthetic airspeed. ret will not be modified
// on failure.
bool synthetic_airspeed(float &ret) const WARN_IF_UNUSED {
ret = _last_airspeed;
bool synthetic_airspeed_EAS(float &ret) const WARN_IF_UNUSED {
ret = _last_airspeed_TAS;
return true;
}
@ -173,12 +173,12 @@ private:
// DCM matrix from the eulers. Called internally we may.
void reset(bool recover_eulers);
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order:
// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order:
// airspeed as filled-in by an enabled airspeed sensor
// if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
// Or if none of the above, fills-in using the previous airspeed estimate
// Return false: if we are using the previous airspeed estimate
bool get_unconstrained_airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const;
bool get_unconstrained_airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const;
// primary representation of attitude of board used for all inertial calculations
Matrix3f _dcm_matrix;
@ -262,7 +262,7 @@ private:
Vector3f _last_fuse;
Vector3f _last_vel;
uint32_t _last_wind_time;
float _last_airspeed;
float _last_airspeed_TAS;
uint32_t _last_consistent_heading;
// estimated wind in m/s

6
libraries/AP_AHRS/AP_AHRS_SIM.cpp
View File

@ -41,7 +41,7 @@ bool AP_AHRS_SIM::wind_estimate(Vector3f &wind) const
return true;
}
bool AP_AHRS_SIM::airspeed_estimate(float &airspeed_ret) const
bool AP_AHRS_SIM::airspeed_EAS(float &airspeed_ret) const
{
if (_sitl == nullptr) {
return false;
@ -52,9 +52,9 @@ bool AP_AHRS_SIM::airspeed_estimate(float &airspeed_ret) const
return true;
}
bool AP_AHRS_SIM::airspeed_estimate(uint8_t index, float &airspeed_ret) const
bool AP_AHRS_SIM::airspeed_EAS(uint8_t index, float &airspeed_ret) const
{
return airspeed_estimate(airspeed_ret);
return airspeed_EAS(airspeed_ret);
}
bool AP_AHRS_SIM::get_quaternion(Quaternion &quat) const

4
libraries/AP_AHRS/AP_AHRS_SIM.h
View File

@ -68,11 +68,11 @@ public:
// return an airspeed estimate if available. return true
// if we have an estimate
bool airspeed_estimate(float &airspeed_ret) const override;
bool airspeed_EAS(float &airspeed_ret) const override;
// return an airspeed estimate if available. return true
// if we have an estimate from a specific sensor index
bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const override;
bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const override;
// return a ground vector estimate in meters/second, in North/East order
Vector2f groundspeed_vector() override;

4
libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h
View File

@ -468,3 +468,7 @@
#ifndef AP_CUSTOMROTATIONS_ENABLED
#define AP_CUSTOMROTATIONS_ENABLED 0
#endif
#ifndef AP_QUICKTUNE_ENABLED
#define AP_QUICKTUNE_ENABLED 0
#endif

17
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -400,17 +400,18 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
}
float engine_mul = _sitl?_sitl->engine_mul.get():1;
uint8_t engine_fail = _sitl?_sitl->engine_fail.get():0;
uint32_t engine_fail = _sitl?_sitl->engine_fail.get():0;
float throttle = 0.0f;
if (engine_fail >= ARRAY_SIZE(input.servos)) {
engine_fail = 0;
}
// apply engine multiplier to motor defined by the SIM_ENGINE_FAIL parameter
if (_vehicle != Rover) {
input.servos[engine_fail] = ((input.servos[engine_fail]-1000) * engine_mul) + 1000;
} else {
input.servos[engine_fail] = static_cast<uint16_t>(((input.servos[engine_fail] - 1500) * engine_mul) + 1500);
for (uint8_t i=0; i<ARRAY_SIZE(input.servos); i++) {
if (engine_fail & (1<<i)) {
if (_vehicle != Rover) {
input.servos[i] = ((input.servos[i]-1000) * engine_mul) + 1000;
} else {
input.servos[i] = static_cast<uint16_t>(((input.servos[i] - 1500) * engine_mul) + 1500);
}
}
}
if (_vehicle == ArduPlane) {

74
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -343,15 +343,11 @@ void AP_InertialSensor_Invensensev3::start()
// pre-calculate backend period
backend_period_us = 1000000UL / backend_rate_hz;
if (!_imu.register_gyro(gyro_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype)) ||
!_imu.register_accel(accel_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype))) {
if (!_imu.register_gyro(gyro_instance, sampling_rate_hz, dev->get_bus_id_devtype(devtype)) ||
!_imu.register_accel(accel_instance, sampling_rate_hz, dev->get_bus_id_devtype(devtype))) {
return;
}
// update backend sample rate
_set_accel_raw_sample_rate(accel_instance, backend_rate_hz);
_set_gyro_raw_sample_rate(gyro_instance, backend_rate_hz);
// indicate what multiplier is appropriate for the sensors'
// readings to fit them into an int16_t:
_set_raw_sample_accel_multiplier(accel_instance, multiplier_accel);
@ -381,16 +377,15 @@ void AP_InertialSensor_Invensensev3::start()
// get a startup banner to output to the GCS
bool AP_InertialSensor_Invensensev3::get_output_banner(char* banner, uint8_t banner_len) {
if (fast_sampling) {
snprintf(banner, banner_len, "IMU%u: fast%s sampling enabled %.1fkHz",
gyro_instance,
snprintf(banner, banner_len, "IMU%u:%s%s sampling %.1fkHz/%.1fkHz",
gyro_instance,
fast_sampling ? " fast" : " normal",
#if HAL_INS_HIGHRES_SAMPLE
highres_sampling ? ", high-resolution" :
highres_sampling ? " hi-res" :
#endif
"" , backend_rate_hz * 0.001);
return true;
}
return false;
"", backend_rate_hz * 0.001,
sampling_rate_hz * 0.001);
return true;
}
/*
@ -669,21 +664,21 @@ void AP_InertialSensor_Invensensev3::register_write_bank(uint8_t bank, uint8_t r
}
// calculate the fast sampling backend rate
uint16_t AP_InertialSensor_Invensensev3::calculate_fast_sampling_backend_rate(uint16_t base_odr, uint16_t max_odr) const
uint16_t AP_InertialSensor_Invensensev3::calculate_fast_sampling_backend_rate(uint16_t base_backend_rate, uint16_t max_backend_rate) const
{
// constrain the gyro rate to be at least the loop rate
uint8_t loop_limit = 1;
if (get_loop_rate_hz() > base_odr) {
loop_limit = 2;
uint8_t min_base_rate_multiplier = 1;
if (get_loop_rate_hz() > base_backend_rate) {
min_base_rate_multiplier = 2;
}
if (get_loop_rate_hz() > base_odr * 2) {
loop_limit = 4;
if (get_loop_rate_hz() > base_backend_rate * 2) {
min_base_rate_multiplier = 4;
}
// constrain the gyro rate to be a 2^N multiple
uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 8);
uint8_t fast_sampling_rate_multiplier = constrain_int16(get_fast_sampling_rate(), min_base_rate_multiplier, 8);
// calculate rate we will be giving samples to the backend
return constrain_int16(base_odr * fast_sampling_rate, base_odr, max_odr);
return constrain_int16(base_backend_rate * fast_sampling_rate_multiplier, base_backend_rate, max_backend_rate);
}
/*
@ -806,6 +801,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
}
}
}
sampling_rate_hz = backend_rate_hz; // sampling rate and backend rate are the same
// disable gyro and accel as per 12.9 in the ICM-42688 docs
register_write(INV3REG_PWR_MGMT0, 0x00);
@ -862,6 +858,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void)
{
backend_rate_hz = 1600;
sampling_rate_hz = 1600;
// use low-noise mode
register_write(INV3REG_70_PWR_MGMT0, 0x0f);
hal.scheduler->delay_microseconds(300);
@ -886,27 +883,24 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void)
*/
void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void)
{
uint8_t odr_config = 4;
backend_rate_hz = 1600;
// always fast sampling
fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
uint8_t odr_config;
backend_rate_hz = 800;
if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() > 1) {
backend_rate_hz = calculate_fast_sampling_backend_rate(backend_rate_hz, backend_rate_hz * 4);
fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() && fast_sampling) {
// For ICM45686 we only set 3200 or 6400Hz as sampling rates
// we don't need to read FIFO faster than requested rate
backend_rate_hz = calculate_fast_sampling_backend_rate(800, 6400);
} else {
fast_sampling = false;
}
// this sensor actually only supports 2 speeds
backend_rate_hz = constrain_int16(backend_rate_hz, 3200, 6400);
switch (backend_rate_hz) {
case 6400: // 6.4Khz
odr_config = 3;
break;
case 3200: // 3.2Khz
odr_config = 4;
break;
default:
break;
if (backend_rate_hz <= 3200) {
odr_config = 0x04; // 3200Hz
sampling_rate_hz = 3200;
} else {
odr_config = 0x03; // 6400Hz
sampling_rate_hz = 6400;
}
// Disable FIFO first

3
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
View File

@ -58,7 +58,7 @@ private:
void set_filter_and_scaling_icm42670(void);
void set_filter_and_scaling_icm456xy(void);
void fifo_reset();
uint16_t calculate_fast_sampling_backend_rate(uint16_t base_odr, uint16_t max_odr) const;
uint16_t calculate_fast_sampling_backend_rate(uint16_t base_backend_rate, uint16_t max_backend_rate) const;
/* Read samples from FIFO */
void read_fifo();
@ -141,4 +141,5 @@ private:
float temp_filtered;
LowPassFilter2pFloat temp_filter;
uint32_t sampling_rate_hz;
};

6
libraries/AP_Networking/AP_Networking_CAN.cpp
View File

@ -182,7 +182,11 @@ void AP_Networking_CAN::mcast_server(void)
const uint16_t timeout_us = 2000;
while (frame_buffers[bus]->peek(frame)) {
auto retcode = get_caniface(bus)->send(frame,
auto *cbus = get_caniface(bus);
if (cbus == nullptr) {
break;
}
auto retcode = cbus->send(frame,
AP_HAL::micros64() + timeout_us,
AP_HAL::CANIface::IsMAVCAN);
if (retcode == 0) {

121
libraries/AP_Scripting/applets/follow-target-send.lua Normal file
View File

@ -0,0 +1,121 @@
-- Send the FOLLOW_TARGET mavlink message to allow other vehicles to follow this one
--
-- How To Use
-- 1. copy this script to the autopilot's "scripts" directory
-- 2. within the "scripts" directory create a "modules" directory
-- 3. copy the MAVLink/mavlink_msgs_xxx files to the "scripts" directory
-- 4. the FOLLOW_TARGET message will be published at 10hz
-- load mavlink message definitions from modules/MAVLink directory
local mavlink_msgs = require("MAVLink/mavlink_msgs")
-- global definitions
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
local UPDATE_INTERVAL_MS = 100 -- update at about 10hz
local FOLLOW_TARGET_CAPABILITIES = {POS=2^0, VEL=2^1, ACCEL=2^2, ATT_RATES=2^3}
-- setup script specific parameters
local PARAM_TABLE_KEY = 88
local PARAM_TABLE_PREFIX = "FOLT_"
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 2), 'could not add param table')
-- add a parameter and bind it to a variable
local function bind_add_param(name, idx, default_value)
assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', PARAM_TABLE_PREFIX .. name))
return Parameter(PARAM_TABLE_PREFIX .. name)
end
--[[
// @Param: FOLT_ENABLE
// @DisplayName: Follow Target Send Enable
// @Description: Follow Target Send Enable
// @Values: 0:Disabled,1:Enabled
// @User: Standard
--]]
local FOLT_ENABLE = bind_add_param("ENABLE", 1, 1)
--[[
// @Param: FOLT_MAV_CHAN
// @DisplayName: Follow Target Send MAVLink Channel
// @Description: MAVLink channel to which FOLLOW_TARGET should be sent
// @Range: 0 10
// @User: Standard
--]]
local FOLT_MAV_CHAN = bind_add_param("MAV_CHAN", 2, 0)
-- initialize MAVLink rx with number of messages, and buffer depth
mavlink:init(2, 5)
-- send FOLLOW_TARGET message
local function send_follow_target_msg()
-- get vehicle location
local curr_loc = ahrs:get_location()
if curr_loc == nil then
do return end
end
local capabilities = FOLLOW_TARGET_CAPABILITIES.POS
-- get vehicle target velocity in m/s in NED frame
local vel_target_NED = poscontrol:get_vel_target()
if vel_target_NED ~= nil then
capabilities = capabilities + FOLLOW_TARGET_CAPABILITIES.VEL
else
vel_target_NED = Vector3f()
end
-- get vehicle target acceleration in m/s/s in NED frame
local accel_target_NED = poscontrol:get_accel_target()
if accel_target_NED ~= nil then
capabilities = capabilities + FOLLOW_TARGET_CAPABILITIES.ACCEL
else
accel_target_NED = Vector3f()
end
-- get vehicle current attitude as quaternion and rates
local attitude_quat = ahrs:get_quaternion()
local curr_rot_rate = ahrs:get_gyro()
if attitude_quat ~= nil and curr_rot_rate ~= nil then
capabilities = capabilities + FOLLOW_TARGET_CAPABILITIES.ATT_RATES
else
attitude_quat = Quaternion()
curr_rot_rate = Vector3f()
end
local curr_rot_rate_NED = ahrs:body_to_earth(curr_rot_rate)
-- prepare FOLLOW_TARGET message
local follow_target_msg = {}
follow_target_msg.timestamp = millis():toint()
follow_target_msg.est_capabilities = capabilities
follow_target_msg.lat = curr_loc:lat()
follow_target_msg.lon = curr_loc:lng()
follow_target_msg.alt = curr_loc:alt() * 0.01
follow_target_msg.vel = {vel_target_NED:x(), vel_target_NED:y(), vel_target_NED:z()}
follow_target_msg.acc = {accel_target_NED:x(), accel_target_NED:y(), accel_target_NED:z()}
follow_target_msg.attitude_q = {attitude_quat:q1(), attitude_quat:q2(), attitude_quat:q3(), attitude_quat:q4()}
follow_target_msg.rates = {curr_rot_rate_NED:x(), curr_rot_rate_NED:y(), curr_rot_rate_NED:z()}
follow_target_msg.position_cov = {0, 0, 0}
follow_target_msg.custom_state = 0
-- send FOLLOW_TARGET message
mavlink:send_chan(FOLT_MAV_CHAN:get(), mavlink_msgs.encode("FOLLOW_TARGET", follow_target_msg))
end
-- display welcome message
gcs:send_text(MAV_SEVERITY.INFO, "follow-target-send script loaded")
-- update function to receive location from payload and move vehicle to reduce payload's oscillation
local function update()
-- exit immediately if not enabled
if (FOLT_ENABLE:get() <= 0) then
return update, 1000
end
-- send FOLLOW_TARGET message
send_follow_target_msg()
return update, UPDATE_INTERVAL_MS
end
return update()

15
libraries/AP_Scripting/applets/follow-target-send.md Normal file
View File

@ -0,0 +1,15 @@
# Follow Target Send
Sends the FOLLOW_TARGET mavlink message to allow other vehicles to follow this one
# Parameters
- FOLT_ENABLE : Set to 1 to enable this script
- FOLT_MAV_CHAN : MAVLink channel to which FOLLOW_TARGET should be sent
# How To Use
1. copy this script to the autopilot's "scripts" directory
2. within the "scripts" directory create a "modules" directory
3. copy the MAVLink/mavlink_msgs_xxx files to the "scripts" directory
4. the FOLLOW_TARGET message will be published at 10hz

8
libraries/AP_Scripting/docs/docs.lua
View File

@ -3785,6 +3785,14 @@ function poscontrol:set_posvelaccel_offset(pos_offset_NED, vel_offset_NED, accel
---@return Vector3f_ud|nil
function poscontrol:get_posvelaccel_offset() end
-- get position controller's target velocity in m/s in NED frame
---@return Vector3f_ud|nil
function poscontrol:get_vel_target() end
-- get position controller's target acceleration in m/s/s in NED frame
---@return Vector3f_ud|nil
function poscontrol:get_accel_target() end
-- desc
AR_PosControl = {}

399
libraries/AP_Scripting/examples/glide_into_wind.lua Normal file
View File

@ -0,0 +1,399 @@
-- Glide into wind, LUA script for glide into wind functionality
-- Background
-- When flying a fixed-wing drone on ad-hoc BVLOS missions, it might not be
-- suitable for the drone to return to home if the C2 link is lost, since that
-- might mean flying without control for an extended time and distance. One
-- option in ArduPlane is to set FS_Long to Glide, which makes the drone glide
-- and land in the direction it happened to have when the command was invoked,
-- without regard to the wind. This script offers a way to decrease the kinetic
-- energy in this blind landing by means of steering the drone towards the wind
-- as GLIDE is initiated, hence lowering the ground speed. The intention is to
-- minimize impact energy at landing - foremost for any third party, but also to
-- minimize damage to the drone.
-- Functionality and setup
-- 1. Set SCR_ENABLE = 1
-- 2. Put script in scripts folder, boot twice
-- 3. A new parameter has appeared:
-- - GLIDE_WIND_ENABL (0=disable, 1=enable)
-- 4. Set GLIDE_WIND_ENABL = 1
-- 5. Read the docs on FS:
-- https://ardupilot.org/plane/docs/apms-failsafe-function.html#failsafe-parameters-and-their-meanings
-- 6. Set FS_LONG_ACTN = 2
-- 7. Set FS_LONG_TIMEOUT as appropriate
-- 8. Set FS_GCS_ENABL = 1
-- 9. If in simulation, set SIM_WIND_SPD = 4 to get a reliable wind direction.
-- 10. Test in simulation: Fly a mission, disable heartbeats by typing 'set
-- heartbeat 0' into mavproxy/SITL, monitor what happens in the console. If
-- QGC or similar GCS is used, make sure it does not send heartbeats.
-- 11. Test in flight: Fly a mission, monitor estimated wind direction from GCS,
-- then fail GCS link and see what happens.
-- 12. Once heading is into wind script will stop steering and not steer again
-- until state machine is reset and failsafe is triggered again. Steering in low
-- airspeeds (thr=0) increases risks of stall and it is preferable touch
-- ground in level roll attitude. If the script parameter hdg_ok_lim is set
-- to tight or the wind estimate is not stable, the script will anyhow stop
-- steering after override_time_lim and enter FBWA - otherwise the script
-- would hinder the GLIDE fail safe.
-- 13. Script will stop interfering as soon as a new goto-point is received or
-- the flight mode is changed by the operator or the remote pilot.
-- During the fail safe maneuver a warning tune is played.
-- State machine
-- CAN_TRIGGER
-- - Do: Nothing
-- - Change state: If the failsafe GLIDE is triggered: if FS_GCS_ENABL is set
-- and FS_LONG_ACTN is 2, change to TRIGGERED else change to CANCELED
--
-- TRIGGERED
-- - Do: First use GUIDED mode to steer into wind, then switch to FBWA to
-- Glide into wind. Play warning tune.
-- - Change state: If flight mode is changed by operator/remote pilot or
-- operator/remote pilot sends a new goto point, change state to CANCELED
--
-- CANCELED
-- - Do: Nothing
-- - Change state: When new heart beat arrive, change state to CAN_TRIGGER
-- Credits
-- This script is developed by agising at UASolutions, commissioned by, and in
-- cooperation with Remote.aero, with funding from Swedish AeroEDIH, in response
-- to a need from the Swedish Sea Rescue Society (Sjöräddningssällskapet, SSRS).
-- Disable diagnostics related to reading parameters to pass linter
---@diagnostic disable: need-check-nil
---@diagnostic disable: param-type-mismatch
-- Tuning parameters
local looptime = 250 -- Short looptime
local long_looptime = 2000 -- Long looptime, GLIDE_WIND is not enabled
local tune_repeat_t = 1000 -- How often to play tune in glide into wind, [ms]
local hdg_ok_lim = 15 -- Acceptable heading error in deg (when to stop steering)
local hdg_ok_t_lim = 5000 -- Stop steering towards wind after hdg_ok_t_lim ms with error less than hdg_ok_lim
local override_time_lim = 15000 -- Max time in GUIDED during GLIDE, after limit set FBWA independent of hdg
-- GCS text levels
local _INFO = 6
local _WARNING = 4
-- Plane flight modes mapping
local mode_FBWA = 5
local mode_GUIDED = 15
-- Tunes
local _tune_glide_warn = "MFT240 L16 cdefgfgfgfg" -- The warning tone played during GLIDE_WIND
--State variable
local fs_state = nil
-- Flags
local override_enable = false -- Flag to allow RC channel loverride
-- Variables
local wind_dir_rad = nil -- param for wind dir in rad
local wind_dir_180 = nil -- param for wind dir in deg
local hdg_error = nil -- Heading error, hdg vs wind_dir_180
local gw_enable = nil -- glide into wind enable flag
local hdg = nil -- vehicle heading
local wind = Vector3f() -- wind 3Dvector
local link_lost_for = nil -- link loss time counter
local last_seen = nil -- timestamp last received heartbeat
local tune_time_since = 0 -- Timer for last played tune
local hdg_ok_t = 0 -- Timer
local expected_flight_mode = nil -- Flight mode set by this script
local location_here = nil -- Current location
local location_upwind = nil -- Location to hold the target location
local user_notified = false -- Flag to keep track user being notified or not
local failed_location_counter = 0 -- Counter for failed location requests, possible GPS denied
local upwind_distance = 500 -- Distance to the upwind location, minimum 4x turn radius
local override_time = 0 -- Time since override started in ms
-- Add param table
local PARAM_TABLE_KEY = 74
local PARAM_TABLE_PREFIX = "GLIDE_WIND_"
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 30), 'could not add param table')
-------
-- Init
-------
function _init()
-- Add and init paramters
GLIDE_WIND_ENABL = bind_add_param('ENABL', 1, 0)
-- Init parameters
FS_GCS_ENABL = bind_param('FS_GCS_ENABL') -- Is set to 1 if GCS lol should trigger FS after FS_LONG_TIMEOUT
FS_LONG_TIMEOUT = bind_param('FS_LONG_TIMEOUT') -- FS long timeout in seconds
FS_LONG_ACTN = bind_param('FS_LONG_ACTN') -- Is set to 2 for Glide
send_to_gcs(_INFO, 'LUA: FS_LONG_TIMEOUT timeout: ' .. FS_LONG_TIMEOUT:get() .. 's')
-- Test paramter
if GLIDE_WIND_ENABL:get() == nil then
send_to_gcs(_INFO, 'LUA: Something went wrong, GLIDE_WIND_ENABL not created')
return _init(), looptime
else
gw_enable = GLIDE_WIND_ENABL:get()
send_to_gcs(_INFO, 'LUA: GLIDE_WIND_ENABL: ' .. gw_enable)
end
-- Init last_seen.
last_seen = gcs:last_seen()
-- Init link_lost_for [ms] to FS_LONG_TIMEOUT [s] to prevent link to recover without
-- new heartbeat. This is to properly init the state machine.
link_lost_for = FS_LONG_TIMEOUT:get() * 1000
-- Warn if GLIDE_WIND_ENABL is set and FS_LONG_ACTN is not GLIDE
if gw_enable == 1 and FS_LONG_ACTN:get() ~= 2 then
send_to_gcs(_WARNING, 'GLIDE_WIND_ENABL is set, but FS_LONG_ACTN is not GLIDE.')
end
-- Init fs_state machine to CANCELED. A heartbeat is required to set the state
-- to CAN_TRIGGER from where Glide into wind can be triggered.
fs_state = 'CANCELED'
-- All set, go to update
return update(), long_looptime
end
------------
-- Main loop
------------
function update()
-- Check if state of GLIDE_WIND_ENABL parameter changed, print every change
if gw_enable ~= GLIDE_WIND_ENABL:get() then
gw_enable = GLIDE_WIND_ENABL:get()
send_to_gcs(_INFO, 'LUA: GLIDE_WIND_ENABL: ' .. gw_enable)
-- If GLIDE_WIND_ENABL was enabled, warn if not FS_LONG_ACTN is set accordingly
if gw_enable == 1 then
if FS_LONG_ACTN:get() ~=2 then
send_to_gcs(_WARNING, 'GLIDE_WIND_ENABL is set, but FS_LONG_ACTN is not GLIDE.')
end
end
end
-- -- If feature is not enabled, loop slowly
if gw_enable == 0 then
return update, long_looptime
end
-- GLIDE_WIND_ENABL is enabled, look for triggers
-- Monitor time since last gcs heartbeat
if last_seen == gcs:last_seen() then
link_lost_for = link_lost_for + looptime
else
-- There has been a new heartbeat, update last_seen and reset link_lost_for
last_seen = gcs:last_seen()
link_lost_for = 0
end
-- Run the state machine
-- State CAN_TRIGGER
if fs_state == 'CAN_TRIGGER' then
if link_lost_for > FS_LONG_TIMEOUT:get() * 1000 then
-- Double check that FS_GCS_ENABL is set
if FS_GCS_ENABL:get() == 1 and FS_LONG_ACTN:get() == 2 then
fs_state = "TRIGGERED"
-- Reset some variables
hdg_ok_t = 0
user_notified = false
override_enable = true
override_time = 0
failed_location_counter = 0
-- Set mode to GUIDED before entering TRIGGERED state
set_flight_mode(mode_GUIDED, 'LUA: Glide into wind state TRIGGERED')
else
-- Do not trigger glide into wind, require new heart beats to get here again
fs_state = "CANCELED"
end
end
-- State TRIGGERED
elseif fs_state == "TRIGGERED" then
-- Check for flight mode changes from outside script
if vehicle:get_mode() ~= expected_flight_mode then
fs_state = "CANCELED"
send_to_gcs(_INFO, 'LUA: Glide into wind state CANCELED: flight mode change')
end
-- In GUIDED, check for target location changes from outside script (operator)
if vehicle:get_mode() == mode_GUIDED then
if not locations_are_equal(vehicle:get_target_location(), location_upwind) then
fs_state = "CANCELED"
send_to_gcs(_INFO, 'LUA: Glide into wind state CANCELED: new goto-point')
end
end
-- State CANCELED
elseif fs_state == "CANCELED" then
-- Await link is not lost
if link_lost_for < FS_LONG_TIMEOUT:get() * 1000 then
fs_state = "CAN_TRIGGER"
send_to_gcs(_INFO, 'LUA: Glide into wind state CAN_TRIGGER')
end
end
-- State TRIGGERED actions
if fs_state == "TRIGGERED" then
-- Get the heading angle
hdg = math.floor(math.deg(ahrs:get_yaw()))
-- Get wind direction. Function wind_estimate returns x and y for direction wind blows in, add pi to get true wind dir
wind = ahrs:wind_estimate()
wind_dir_rad = math.atan(wind:y(), wind:x())+math.pi
wind_dir_180 = math.floor(wrap_180(math.deg(wind_dir_rad)))
hdg_error = wrap_180(wind_dir_180 - hdg)
-- Check if we are close to target heading
if math.abs(hdg_error) < hdg_ok_lim then
-- If we have been close to target heading for hdg_ok_t_lim, switch back to FBWA
if hdg_ok_t > hdg_ok_t_lim then
if override_enable then
set_flight_mode(mode_FBWA,'LUA: Glide into wind steering complete, GLIDE in FBWA')
end
-- Do not override again until state machine has triggered again
override_enable = false
else
hdg_ok_t = hdg_ok_t + looptime
end
-- Heading error is big, reset timer hdg_ok_t
else
hdg_ok_t = 0
end
-- Play tune every tune_repeat_t [ms]
if tune_time_since > tune_repeat_t then
-- Play tune and reset timer
send_to_gcs(_INFO, 'LUA: Play warning tune')
play_tune(_tune_glide_warn)
tune_time_since = 0
else
tune_time_since = tune_time_since + looptime
end
-- If not steered into wind yet, update goto point into wind
if override_enable then
-- Check override time, if above limit, switch back to FBWA
override_time = override_time + looptime
if override_time > override_time_lim then
set_flight_mode(mode_FBWA, "LUA: Glide into wind override time out, GLIDE in current heading")
override_enable = false
end
-- Get current position and handle if not valid
location_here = ahrs:get_location()
if location_here == nil then
-- In case we cannot get location for some time we must give up and continue with GLIDE
failed_location_counter = failed_location_counter + 1
if failed_location_counter > 5 then
set_flight_mode(mode_FBWA, "LUA: Glide failed to get location, GLIDE in current heading")
override_enable = false
return update, looptime
end
gcs:send_text(_WARNING, "LUA: Glide failed to get location")
return update, looptime
end
-- Calc upwind position, copy and modify location_here
location_upwind = location_here:copy()
location_upwind:offset_bearing(wind_dir_180, upwind_distance)
-- Set location_upwind as GUIDED target
if vehicle:set_target_location(location_upwind) then
if not user_notified then
send_to_gcs(_INFO, "LUA: Guided target set " .. upwind_distance .. "m away at bearing " .. wind_dir_180)
-- Just notify once
user_notified = true
end
else
-- Most likely we are not in GUIDED anymore (operator changed mode), state machine will handle this in next loop.
gcs:send_text(_WARNING, "LUA: Glide failed to set upwind target")
end
end
end
return update, looptime
end
-------------------
-- Helper functions
-------------------
-- Set mode and wait for mode change
function set_flight_mode(mode, message)
expected_flight_mode = mode
vehicle:set_mode(expected_flight_mode)
return wait_for_mode_change(mode, message, 0)
end
-- Wait for mode change
function wait_for_mode_change(mode, message, attempt)
-- If mode change does not go through after 10 attempts, give up
if attempt > 10 then
send_to_gcs(_WARNING, 'LUA: Glide into wind mode change failed.')
return update, looptime
-- If mode change does not go through, wait and try again
elseif vehicle:get_mode() ~= mode then
return wait_for_mode_change(mode, message, attempt + 1), 5
-- Mode change has gone through
else
send_to_gcs(_INFO, message)
return update, looptime
end
end
-- Function to compare two Location objects
function locations_are_equal(loc1, loc2)
-- If either location is nil, they are not equal
if not loc1 or not loc2 then
return false
end
-- Compare latitude and longitude, return bool
return loc1:lat() == loc2:lat() and loc1:lng() == loc2:lng()
end
-- bind a parameter to a variable
function bind_param(name)
local p = Parameter()
assert(p:init(name), string.format('could not find %s parameter', name))
return p
end
-- Add a parameter and bind it to a variable
function bind_add_param(name, idx, default_value)
assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
return bind_param(PARAM_TABLE_PREFIX .. name)
end
-- Print to GCS
function send_to_gcs(level, mess)
gcs:send_text(level, mess)
end
-- Play tune
function play_tune(tune)
notify:play_tune(tune)
end
-- Returns the angle in range 0-360
function wrap_360(angle)
local res = math.fmod(angle, 360.0)
if res < 0 then
res = res + 360.0
end
return res
end
-- Returns the angle in range -180-180
function wrap_180(angle)
local res = wrap_360(angle)
if res > 180 then
res = res - 360
end
return res
end
-- Start up the script
return _init, 2000

2
libraries/AP_Scripting/generator/description/bindings.desc
View File

@ -774,6 +774,8 @@ singleton AC_PosControl depends APM_BUILD_COPTER_OR_HELI
singleton AC_PosControl rename poscontrol
singleton AC_PosControl method set_posvelaccel_offset boolean Vector3f Vector3f Vector3f
singleton AC_PosControl method get_posvelaccel_offset boolean Vector3f'Null Vector3f'Null Vector3f'Null
singleton AC_PosControl method get_vel_target boolean Vector3f'Null
singleton AC_PosControl method get_accel_target boolean Vector3f'Null
include APM_Control/AR_AttitudeControl.h depends APM_BUILD_TYPE(APM_BUILD_Rover)
singleton AR_AttitudeControl depends APM_BUILD_TYPE(APM_BUILD_Rover)

16
libraries/AP_Scripting/modules/MAVLink/mavlink_msg_FOLLOW_TARGET.lua Normal file
View File

@ -0,0 +1,16 @@
local FOLLOW_TARGET = {}
FOLLOW_TARGET.id = 144
FOLLOW_TARGET.fields = {
{ "timestamp", "<I8" },
{ "custom_state", "<I8" },
{ "lat", "<i4" },
{ "lon", "<i4" },
{ "alt", "<f" },
{ "vel", "<f", 3 },
{ "acc", "<f", 3 },
{ "attitude_q", "<f", 4 },
{ "rates", "<f", 3 },
{ "position_cov", "<f", 3 },
{ "est_capabilities", "<B" },
}
return FOLLOW_TARGET

3
libraries/AP_Volz_Protocol/AP_Volz_Protocol.h
View File

@ -88,7 +88,6 @@ private:
// Loop in thread to output to uart
void loop();
uint8_t sent_count;
void init(void);
@ -120,6 +119,8 @@ private:
void read_telem();
void process_response(const CMD &cmd);
uint8_t sent_count;
struct {
CMD_ID types[3] {
CMD_ID::READ_CURRENT,

1
libraries/GCS_MAVLink/GCS.h
View File

@ -403,7 +403,6 @@ public:
#if AP_WINCH_ENABLED
virtual void send_winch_status() const {};
#endif
void send_water_depth() const;
int8_t battery_remaining_pct(const uint8_t instance) const;
#if HAL_HIGH_LATENCY2_ENABLED

58
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -5966,57 +5966,6 @@ void GCS_MAVLINK::send_generator_status() const
}
#endif
#if AP_RANGEFINDER_ENABLED && APM_BUILD_TYPE(APM_BUILD_Rover)
void GCS_MAVLINK::send_water_depth() const
{
if (!HAVE_PAYLOAD_SPACE(chan, WATER_DEPTH)) {
return;
}
RangeFinder *rangefinder = RangeFinder::get_singleton();
if (rangefinder == nullptr || !rangefinder->has_orientation(ROTATION_PITCH_270)){
return;
}
// get position
const AP_AHRS &ahrs = AP::ahrs();
Location loc;
IGNORE_RETURN(ahrs.get_location(loc));
for (uint8_t i=0; i<rangefinder->num_sensors(); i++) {
const AP_RangeFinder_Backend *s = rangefinder->get_backend(i);
if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) {
continue;
}
// get temperature
float temp_C;
if (!s->get_temp(temp_C)) {
temp_C = 0.0f;
}
const bool sensor_healthy = (s->status() == RangeFinder::Status::Good);
mavlink_msg_water_depth_send(
chan,
AP_HAL::millis(), // time since system boot TODO: take time of measurement
i, // rangefinder instance
sensor_healthy, // sensor healthy
loc.lat, // latitude of vehicle
loc.lng, // longitude of vehicle
loc.alt * 0.01f, // altitude of vehicle (MSL)
ahrs.get_roll(), // roll in radians
ahrs.get_pitch(), // pitch in radians
ahrs.get_yaw(), // yaw in radians
s->distance(), // distance in meters
temp_C); // temperature in degC
}
}
#endif // AP_RANGEFINDER_ENABLED && APM_BUILD_TYPE(APM_BUILD_Rover)
#if HAL_ADSB_ENABLED
void GCS_MAVLINK::send_uavionix_adsb_out_status() const
{
@ -6547,13 +6496,6 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
break;
#endif
#if AP_RANGEFINDER_ENABLED && APM_BUILD_TYPE(APM_BUILD_Rover)
case MSG_WATER_DEPTH:
CHECK_PAYLOAD_SIZE(WATER_DEPTH);
send_water_depth();
break;
#endif
#if HAL_HIGH_LATENCY2_ENABLED
case MSG_HIGH_LATENCY2:
CHECK_PAYLOAD_SIZE(HIGH_LATENCY2);

6
libraries/SITL/SITL.cpp
View File

@ -78,8 +78,8 @@ const AP_Param::GroupInfo SIM::var_info[] = {
// @Param: ENGINE_MUL
// @DisplayName: Engine failure thrust scaler
// @Description: Thrust from Motors in SIM_ENGINE_FAIL will be multiplied by this factor
// @Units: ms
AP_GROUPINFO("ENGINE_MUL", 8, SIM, engine_mul, 1),
// @Range: 0 1
AP_GROUPINFO("ENGINE_MUL", 8, SIM, engine_mul, 0),
// @Param: WIND_SPD
// @DisplayName: Simulated Wind speed
// @Description: Allows you to emulate wind in sim
@ -246,7 +246,7 @@ const AP_Param::GroupInfo SIM::var_info[] = {
// @Param: ENGINE_FAIL
// @DisplayName: Engine Fail Mask
// @Description: mask of motors which SIM_ENGINE_MUL will be applied to
// @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8
// @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15, 15: Servo 16, 16: Servo 17, 17: Servo 18, 18: Servo 19, 19: Servo 20, 20: Servo 21, 21: Servo 22, 22: Servo 23, 23: Servo 24, 24: Servo 25, 25: Servo 26, 26: Servo 27, 27: Servo 28, 28: Servo 29, 29: Servo 30, 30: Servo 31, 31: Servo 32
AP_GROUPINFO("ENGINE_FAIL", 58, SIM, engine_fail, 0),
AP_SUBGROUPINFO(models, "", 59, SIM, SIM::ModelParm),
AP_SUBGROUPEXTENSION("", 60, SIM, var_mag),

2
libraries/SITL/SITL.h
View File

@ -197,7 +197,7 @@ public:
AP_Float drift_speed; // degrees/second/minute
AP_Float drift_time; // period in minutes
AP_Float engine_mul; // engine multiplier
AP_Int8 engine_fail; // engine servo to fail (0-7)
AP_Int32 engine_fail; // mask of engine/motor servo outputs to fail
// initial offset on GPS lat/lon, used to shift origin
AP_Float gps_init_lat_ofs;

2
libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param
View File

@ -612,7 +612,7 @@ SIM_BATT_VOLTAGE,12.6
SIM_DRIFT_SPEED,0.05
SIM_DRIFT_TIME,5
SIM_ENGINE_FAIL,0
SIM_ENGINE_MUL,1
SIM_ENGINE_MUL,0
SIM_FLOAT_EXCEPT,1
SIM_FLOW_DELAY,0
SIM_FLOW_ENABLE,0

2
libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param
View File

@ -632,7 +632,7 @@ SIM_BATT_VOLTAGE,12.6
SIM_DRIFT_SPEED,0.05
SIM_DRIFT_TIME,5
SIM_ENGINE_FAIL,0
SIM_ENGINE_MUL,1
SIM_ENGINE_MUL,0
SIM_FLOAT_EXCEPT,1
SIM_FLOW_DELAY,0
SIM_FLOW_ENABLE,0