mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 19:43:34 +08:00
stop manually defining physical constants
This commit is contained in:
committed by
Lorenz Meier
parent
7538ea44e3
commit
d75fd72c02
@@ -1918,7 +1918,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
|
||||
if (orb_copy(ORB_ID(sensor_baro), _sensor_baro_sub, &baro) == PX4_OK) {
|
||||
if (PX4_ISFINITE(baro.pressure) && PX4_ISFINITE(_parameters.throttle_alt_scale)) {
|
||||
// scale throttle as a function of sqrt(p0/p) (~ EAS -> TAS at low speeds and altitudes ignoring temperature)
|
||||
const float eas2tas = sqrtf(MSL_PRESSURE_MILLIBAR / baro.pressure);
|
||||
const float eas2tas = sqrtf(CONSTANTS_STD_PRESSURE_MBAR / baro.pressure);
|
||||
const float scale = constrain((eas2tas - 1.0f) * _parameters.throttle_alt_scale + 1.0f, 1.0f, 2.0f);
|
||||
|
||||
throttle_max = constrain(throttle_max * scale, throttle_min, 1.0f);
|
||||
|
||||
@@ -103,8 +103,6 @@ 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;
|
||||
|
||||
static constexpr float MSL_PRESSURE_MILLIBAR = 1013.25f; ///< standard atmospheric pressure in millibar
|
||||
|
||||
using math::constrain;
|
||||
using math::max;
|
||||
using math::min;
|
||||
|
||||
@@ -775,7 +775,7 @@ void BlockLocalPositionEstimator::predict()
|
||||
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
|
||||
// note, bias is removed in dynamics function
|
||||
_u = _R_att * a;
|
||||
_u(U_az) += 9.81f; // add g
|
||||
_u(U_az) += CONSTANTS_ONE_G; // add g
|
||||
|
||||
// update state space based on new states
|
||||
updateSSStates();
|
||||
|
||||
@@ -90,8 +90,6 @@
|
||||
#include "mavlink_main.h"
|
||||
#include "mavlink_command_sender.h"
|
||||
|
||||
static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
|
||||
|
||||
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
||||
_mavlink(parent),
|
||||
_mission_manager(nullptr),
|
||||
@@ -1922,9 +1920,9 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
|
||||
struct accel_report accel = {};
|
||||
|
||||
accel.timestamp = timestamp;
|
||||
accel.x_raw = imu.xacc / mg2ms2;
|
||||
accel.y_raw = imu.yacc / mg2ms2;
|
||||
accel.z_raw = imu.zacc / mg2ms2;
|
||||
accel.x_raw = imu.xacc / (CONSTANTS_ONE_G / 1000.0f);
|
||||
accel.y_raw = imu.yacc / (CONSTANTS_ONE_G / 1000.0f);
|
||||
accel.z_raw = imu.zacc / (CONSTANTS_ONE_G / 1000.0f);
|
||||
accel.x = imu.xacc;
|
||||
accel.y = imu.yacc;
|
||||
accel.z = imu.zacc;
|
||||
|
||||
@@ -52,11 +52,6 @@ extern "C" __EXPORT hrt_abstime hrt_reset(void);
|
||||
#define SEND_INTERVAL 20
|
||||
#define UDP_PORT 14560
|
||||
|
||||
#define PRESS_GROUND 101325.0f
|
||||
#define DENSITY 1.2041f
|
||||
|
||||
static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
|
||||
|
||||
#ifdef ENABLE_UART_RC_INPUT
|
||||
#ifndef B460800
|
||||
#define B460800 460800
|
||||
@@ -632,12 +627,12 @@ void Simulator::initializeSensorData()
|
||||
{
|
||||
// write sensor data to memory so that drivers can copy data from there
|
||||
RawMPUData mpu = {};
|
||||
mpu.accel_z = 9.81f;
|
||||
mpu.accel_z = CONSTANTS_ONE_G;
|
||||
|
||||
write_MPU_data(&mpu);
|
||||
|
||||
RawAccelData accel = {};
|
||||
accel.z = 9.81f;
|
||||
accel.z = CONSTANTS_ONE_G;
|
||||
|
||||
write_accel_data(&accel);
|
||||
|
||||
@@ -1036,9 +1031,9 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
|
||||
struct accel_report accel = {};
|
||||
|
||||
accel.timestamp = timestamp;
|
||||
accel.x_raw = imu->xacc / mg2ms2;
|
||||
accel.y_raw = imu->yacc / mg2ms2;
|
||||
accel.z_raw = imu->zacc / mg2ms2;
|
||||
accel.x_raw = imu->xacc / (CONSTANTS_ONE_G / 1000.0f);
|
||||
accel.y_raw = imu->yacc / (CONSTANTS_ONE_G / 1000.0f);
|
||||
accel.z_raw = imu->zacc / (CONSTANTS_ONE_G / 1000.0f);
|
||||
accel.x = imu->xacc;
|
||||
accel.y = imu->yacc;
|
||||
accel.z = imu->zacc;
|
||||
|
||||
@@ -32,7 +32,7 @@
|
||||
############################################################################
|
||||
|
||||
set(SRCS
|
||||
airspeed.c
|
||||
airspeed.cpp
|
||||
battery.cpp
|
||||
board_serial.c
|
||||
bson/tinybson.c
|
||||
|
||||
@@ -236,7 +236,7 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp
|
||||
{
|
||||
float density = get_air_density(static_pressure, temperature_celsius);
|
||||
|
||||
if (density < 0.0001f || !isfinite(density)) {
|
||||
if (density < 0.0001f || !PX4_ISFINITE(density)) {
|
||||
density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user