From 4173d5913906dc2707b045c2978473a555cb2652 Mon Sep 17 00:00:00 2001 From: Jacob Moore Date: Tue, 17 Dec 2024 08:31:27 -0700 Subject: [PATCH] removed air density as a parameter. Using ideal gas law --- include/mixer.h | 1 - include/param.h | 2 -- include/sensors.h | 3 +++ src/controller.cpp | 4 +--- src/mixer.cpp | 6 +++--- src/param.cpp | 2 -- src/sensors.cpp | 2 ++ 7 files changed, 9 insertions(+), 11 deletions(-) diff --git a/include/mixer.h b/include/mixer.h index d884c445..b1a53562 100644 --- a/include/mixer.h +++ b/include/mixer.h @@ -274,7 +274,6 @@ class Mixer : public ParamListenerInterface // Define parameters for the mixer float R_; // Motor resistance - float rho_; // Air density float K_V_; // Motor back-emf constant float K_Q_; // Motor torque constant float i_0_; // Motor no-load current diff --git a/include/param.h b/include/param.h index 1a61933b..3a082b93 100644 --- a/include/param.h +++ b/include/param.h @@ -62,8 +62,6 @@ enum : uint16_t PARAM_BAUD_RATE = 0, PARAM_SERIAL_DEVICE, - PARAM_AIR_DENSITY, - PARAM_NUM_MOTORS, PARAM_MOTOR_RESISTANCE, PARAM_MOTOR_KV, diff --git a/include/sensors.h b/include/sensors.h index 6f41f1e4..526fef4c 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -177,6 +177,7 @@ class Sensors : public ParamListenerInterface Sensors(ROSflight & rosflight); inline const Data & data() const { return data_; } + inline float rho() { return rho_; } void get_filtered_IMU(turbomath::Vector & accel, turbomath::Vector & gyro, uint64_t & stamp_us); // function declarations @@ -227,6 +228,8 @@ class Sensors : public ParamListenerInterface float accel_[3] = {0, 0, 0}; float gyro_[3] = {0, 0, 0}; + float rho_ = 1.225; + bool calibrating_acc_flag_ = false; bool calibrating_gyro_flag_ = false; uint8_t next_sensor_to_update_ = BAROMETER; diff --git a/src/controller.cpp b/src/controller.cpp index 4d642017..a2f8c584 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -79,7 +79,7 @@ void Controller::calculate_max_thrust() { float R = RF_.params_.get_param_float(PARAM_MOTOR_RESISTANCE); float D = RF_.params_.get_param_float(PARAM_PROP_DIAMETER); - float rho = RF_.params_.get_param_float(PARAM_AIR_DENSITY); + float rho = RF_.sensors_.rho(); float CQ = RF_.params_.get_param_float(PARAM_PROP_CQ); float CT = RF_.params_.get_param_float(PARAM_PROP_CT); float KV = RF_.params_.get_param_float(PARAM_MOTOR_KV); @@ -210,8 +210,6 @@ void Controller::param_change_callback(uint16_t param_id) case PARAM_PID_YAW_RATE_D: case PARAM_MOTOR_RESISTANCE: case PARAM_PROP_DIAMETER: - case PARAM_AIR_DENSITY: - case PARAM_PROP_CQ: case PARAM_PROP_CT: case PARAM_MOTOR_KV: case PARAM_NO_LOAD_CURRENT: diff --git a/src/mixer.cpp b/src/mixer.cpp index 7606905c..459f0526 100644 --- a/src/mixer.cpp +++ b/src/mixer.cpp @@ -219,7 +219,6 @@ void Mixer::param_change_callback(uint16_t param_id) init_mixing(); break; case PARAM_MOTOR_RESISTANCE: - case PARAM_AIR_DENSITY: case PARAM_MOTOR_KV: case PARAM_NO_LOAD_CURRENT: case PARAM_PROP_DIAMETER: @@ -325,7 +324,6 @@ void Mixer::init_mixing() void Mixer::update_parameters() { R_ = RF_.params_.get_param_float(PARAM_MOTOR_RESISTANCE); - rho_ = RF_.params_.get_param_float(PARAM_AIR_DENSITY); K_V_ = RF_.params_.get_param_float(PARAM_MOTOR_KV); K_Q_ = K_V_; i_0_ = RF_.params_.get_param_float(PARAM_NO_LOAD_CURRENT); @@ -610,6 +608,8 @@ float Mixer::mix_multirotor_with_motor_parameters(Controller::Output commands) // Mix the inputs float max_output = 1.0; + float rho = RF_.sensors_.rho(); + for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) { if ((*mixer_to_use_.output_type)[i] == M) { // Matrix multiply to mix outputs for Motor type @@ -625,7 +625,7 @@ float Mixer::mix_multirotor_with_motor_parameters(Controller::Output commands) // Ch. 4, setting equation for torque produced by a propeller equal to Eq. 4.19 // Note that we assume constant advance ratio, leading to constant torque and thrust constants. - float V_in = rho_ * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_ + float V_in = rho * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_ + R_ * i_0_ + K_V_ * sqrt(omega_squared); // Convert desired V_in setting to a throttle setting diff --git a/src/param.cpp b/src/param.cpp index c693ec22..bb80f718 100644 --- a/src/param.cpp +++ b/src/param.cpp @@ -108,8 +108,6 @@ void Params::set_defaults(void) init_param_int(PARAM_BAUD_RATE, "BAUD_RATE", 921600); // Baud rate of MAVlink communication with companion computer | 9600 | 921600 init_param_int(PARAM_SERIAL_DEVICE, "SERIAL_DEVICE", 0); // Serial Port (for supported devices) | 0 | 3 - init_param_float(PARAM_AIR_DENSITY, "AIR_DENSITY", 1.225f); // Density of the air (kg/m^3) | 0 | 1000.0 - init_param_int(PARAM_NUM_MOTORS, "NUM_MOTORS", 4); // Number of vertical-facing motors on the vehicle | 1 | 8 init_param_float(PARAM_MOTOR_RESISTANCE, "MOTOR_RESISTANCE", 0.042f); // Electrical resistance of the motor windings (ohms) | 0 | 1000.0 init_param_float(PARAM_MOTOR_KV, "MOTOR_KV", 0.01706f); // Back emf constant of the motor in SI units (V/rad/s) | 0 | 1000.0 diff --git a/src/sensors.cpp b/src/sensors.cpp index fb491836..6dad39dd 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -174,6 +174,8 @@ got_flags Sensors::run() got.baro = true; rf_.board_.baro_read(&data_.baro_pressure, &data_.baro_temperature); correct_baro(); + + rho_ = (0.0289644 * data_.baro_pressure) / (8.31432 * data_.baro_temperature); } }