Skip to content

Commit

Permalink
SITL: document airspeed params
Browse files Browse the repository at this point in the history
  • Loading branch information
antholuo committed Sep 20, 2023
1 parent c352de2 commit 4591aae
Show file tree
Hide file tree
Showing 2 changed files with 58 additions and 21 deletions.
35 changes: 14 additions & 21 deletions libraries/SITL/SITL.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,11 @@ const AP_Param::GroupInfo SIM::var_info[] = {
// @Range: 1 10
// @User: Advanced
AP_GROUPINFO("SPEEDUP", 52, SIM, speedup, -1),
// @Param: IMU_POS
// @DisplayName: IMU Offsets
// @Description: XYZ position of the IMU accelerometer relative to the body frame origin
// @Units: m
// @Vector3Parameter: 1
AP_GROUPINFO("IMU_POS", 53, SIM, imu_pos_offset, 0),
AP_SUBGROUPEXTENSION("", 54, SIM, var_ins),
AP_GROUPINFO("SONAR_POS", 55, SIM, rngfnd_pos_offset, 0),
Expand Down Expand Up @@ -353,7 +358,9 @@ const AP_Param::GroupInfo SIM::var_info3[] = {

AP_GROUPINFO("RATE_HZ", 22, SIM, loop_rate_hz, SIM_RATE_HZ_DEFAULT),

// count of simulated IMUs
// @Param: IMU_COUNT
// @DisplayName: IMU count
// @Description: Number of simulated IMUs to create
AP_GROUPINFO("IMU_COUNT", 23, SIM, imu_count, 2),

// @Path: ./SIM_FETtecOneWireESC.cpp
Expand Down Expand Up @@ -432,9 +439,13 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
// @User: Advanced
AP_GROUPINFO("UART_LOSS", 42, SIM, uart_byte_loss_pct, 0),

AP_SUBGROUPINFO(airspeed[0], "ARSPD_", 50, SIM, SIM::AirspeedParm),
// @Group: ARSPD_
// @Path: ./SITL_Airspeed.cpp
AP_SUBGROUPINFO(airspeed[0], "ARSPD_", 50, SIM, AirspeedParm),
#if AIRSPEED_MAX_SENSORS > 1
AP_SUBGROUPINFO(airspeed[1], "ARSPD2_", 51, SIM, SIM::AirspeedParm),
// @Group: ARSPD2_
// @Path: ./SITL_Airspeed.cpp
AP_SUBGROUPINFO(airspeed[1], "ARSPD2_", 51, SIM, AirspeedParm),
#endif


Expand Down Expand Up @@ -464,24 +475,6 @@ const AP_Param::GroupInfo SIM::BaroParm::var_info[] = {
AP_GROUPEND
};

// user settable parameters for airspeed sensors
const AP_Param::GroupInfo SIM::AirspeedParm::var_info[] = {
// user settable parameters for the 1st airspeed sensor
AP_GROUPINFO("RND", 1, SIM::AirspeedParm, noise, 2.0),
AP_GROUPINFO("OFS", 2, SIM::AirspeedParm, offset, 2013),
// @Param: ARSPD_FAIL
// @DisplayName: Airspeed sensor failure
// @Description: Simulates Airspeed sensor 1 failure
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO("FAIL", 3, SIM::AirspeedParm, fail, 0),
AP_GROUPINFO("FAILP", 4, SIM::AirspeedParm, fail_pressure, 0),
AP_GROUPINFO("PITOT", 5, SIM::AirspeedParm, fail_pitot_pressure, 0),
AP_GROUPINFO("SIGN", 6, SIM::AirspeedParm, signflip, 0),
AP_GROUPINFO("RATIO", 7, SIM::AirspeedParm, ratio, 1.99),
AP_GROUPEND
};

#if HAL_SIM_GPS_ENABLED
// GPS SITL parameters
const AP_Param::GroupInfo SIM::var_gps[] = {
Expand Down
44 changes: 44 additions & 0 deletions libraries/SITL/SITL_Airspeed.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
#include "SITL.h"

#if AP_SIM_ENABLED

namespace SITL {
// user settable parameters for airspeed sensors
const AP_Param::GroupInfo SIM::AirspeedParm::var_info[] = {
// user settable parameters for the 1st airspeed sensor
AP_GROUPINFO("RND", 1, AirspeedParm, noise, 2.0),
AP_GROUPINFO("OFS", 2, AirspeedParm, offset, 2013),
// @Param: FAIL
// @DisplayName: Airspeed sensor failure
// @Description: Simulates Airspeed sensor 1 failure
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO("FAIL", 3, AirspeedParm, fail, 0),
// @Param: FAILP
// @DisplayName: Airspeed sensor failure pressure
// @Description: Simulated airspeed sensor failure pressure
// @Units: Pa
// @User: Advanced
AP_GROUPINFO("FAILP", 4, AirspeedParm, fail_pressure, 0),
// @Param: PITOT
// @DisplayName: Airspeed pitot tube failure pressure
// @Description: Simulated airspeed sensor pitot tube failure pressure
// @Units: Pa
// @User: Advanced
AP_GROUPINFO("PITOT", 5, AirspeedParm, fail_pitot_pressure, 0),
// @Param: SIGN
// @DisplayName: Airspeed signflip
// @Description: Simulated airspeed sensor with reversed pitot/static connections
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO("SIGN", 6, AirspeedParm, signflip, 0),
// @Param: RATIO
// @DisplayName: Airspeed ratios
// @Description: Simulated airspeed sensor ratio
// @User: Advanced
AP_GROUPINFO("RATIO", 7, AirspeedParm, ratio, 1.99),
AP_GROUPEND
};
}

#endif // AP_SIM_ENABLED

0 comments on commit 4591aae

Please sign in to comment.