diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index 7e5a3aef28..9bc3791dd2 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -21,6 +21,23 @@ #include #include +#include + +/* + use picojson to load optional frame files + */ +#define PICOJSON_NOEXCEPT +#ifndef PICOJSON_ASSERT +#define PICOJSON_ASSERT(e) \ + do { \ + if (!(e)) \ + ::printf(#e "\n"); \ + } while (0) +#endif +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#include "picojson.h" +#pragma GCC diagnostic pop using namespace SITL; @@ -278,16 +295,160 @@ static Frame supported_frames[] = Frame("firefly", 6, firefly_motors) }; -void Frame::init(float _mass, float _hover_throttle, float _terminal_velocity, float _terminal_rotation_rate) +// get air density in kg/m^3 +float Frame::get_air_density(float alt_amsl) const { - /* - scaling from total motor power to Newtons. Allows the copter - to hover against gravity when each motor is at hover_throttle - */ - thrust_scale = (_mass * GRAVITY_MSS) / (num_motors * _hover_throttle); + float sigma, delta, theta; - terminal_velocity = _terminal_velocity; - terminal_rotation_rate = _terminal_rotation_rate; + AP_Baro::SimpleAtmosphere(alt_amsl * 0.001f, sigma, delta, theta); + + const float air_pressure = SSL_AIR_PRESSURE * delta; + return air_pressure / (ISA_GAS_CONSTANT * (C_TO_KELVIN + model.refTempC)); +} + +/* + load frame specific parameters from a json file if available + */ +void Frame::load_frame_params(const char *model_json) +{ + ::printf("Loading model %s\n", model_json); + int fd = open(model_json, O_RDONLY); + if (fd == -1) { + AP_HAL::panic("%s failed to load\n", model_json); + } + struct stat st; + if (fstat(fd, &st) != 0) { + AP_HAL::panic("%s failed to load\n", model_json); + } + char buf[st.st_size]; + if (read(fd, buf, st.st_size) != st.st_size) { + AP_HAL::panic("%s failed to load\n", model_json); + } + close(fd); + + char *start = strchr(buf, '{'); + if (!start) { + AP_HAL::panic("Invalid json %s", model_json); + } + + /* + remove comments, as not allowed by the parser + */ + for (char *p = strchr(start,'#'); p; p=strchr(p+1, '#')) { + // clear to end of line + do { + *p++ = ' '; + } while (*p != '\n' && *p != '\r' && *p); + } + + picojson::value obj; + std::string err = picojson::parse(obj, start); + if (!err.empty()) { + AP_HAL::panic("Failed to load %s: %s", model_json, err.c_str()); + exit(1); + } + + struct { + const char *label; + float &v; + } vars[] = { +#define FRAME_VAR(s) { #s, model.s } + FRAME_VAR(mass), + FRAME_VAR(diagonal_size), + FRAME_VAR(refSpd), + FRAME_VAR(refAngle), + FRAME_VAR(refVoltage), + FRAME_VAR(refCurrent), + FRAME_VAR(refAlt), + FRAME_VAR(refTempC), + FRAME_VAR(maxVoltage), + FRAME_VAR(refBatRes), + FRAME_VAR(propExpo), + FRAME_VAR(refRotRate), + FRAME_VAR(hoverThrOut), + FRAME_VAR(pwmMin), + FRAME_VAR(pwmMax), + FRAME_VAR(spin_min), + FRAME_VAR(spin_max), + FRAME_VAR(slew_max), + }; + static_assert(sizeof(model) == sizeof(float)*ARRAY_SIZE(vars), "incorrect model vars"); + + for (uint8_t i=0; i()) { + // use default value + continue; + } + if (!v.is()) { + AP_HAL::panic("Bad json type for %s: %s", vars[i].label, v.to_str().c_str()); + } + vars[i].v = v.get(); + } + + ::printf("Loaded model params from %s\n", model_json); +} + +/* + initialise the frame + */ +void Frame::init(const char *frame_str) +{ + model = default_model; + + const char *colon = strchr(frame_str, ':'); + size_t slen = strlen(frame_str); + if (colon != nullptr && slen > 5 && strcmp(&frame_str[slen-5], ".json") == 0) { + load_frame_params(colon+1); + } + mass = model.mass; + + const float drag_force = model.mass * GRAVITY_MSS * tanf(radians(model.refAngle)); + + float ref_air_density = get_air_density(model.refAlt); + + areaCd = drag_force / (0.5 * ref_air_density * sq(model.refSpd)); + + terminal_rotation_rate = model.refRotRate; + + float hover_thrust = mass * GRAVITY_MSS; + float hover_power = model.refCurrent * model.refVoltage; + float hover_velocity_out = 2 * hover_power / hover_thrust; + float effective_disc_area = hover_thrust / (0.5 * ref_air_density * sq(hover_velocity_out)); + velocity_max = hover_velocity_out / sqrtf(model.hoverThrOut); + thrust_max = 0.5 * ref_air_density * effective_disc_area * sq(velocity_max); + effective_prop_area = effective_disc_area / num_motors; + + // power_factor is ratio of power consumed per newton of thrust + float power_factor = hover_power / hover_thrust; + + // init voltage + voltage_filter.reset(AP::sitl()->batt_voltage); + + for (uint8_t i=0; ivibe_motor)) { - rpm[i] = sqrtf(mthrust.length() / scaling) * AP::sitl()->vibe_motor * 60.0f; + const float mot_thrust_max = thrust_max / num_motors; + rpm[i] = sqrtf(mthrust.length() / mot_thrust_max) * AP::sitl()->vibe_motor * 60.0f; } } @@ -339,16 +504,25 @@ void Frame::calculate_forces(const Aircraft &aircraft, rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate; } - if (terminal_velocity > 0) { - // air resistance - Vector3f air_resistance = -aircraft.get_velocity_air_ef() * (GRAVITY_MSS/terminal_velocity); - body_accel += aircraft.get_dcm().transposed() * air_resistance; + if (use_drag) { + // use the model params to calculate drag + const Vector3f vel_air_ef = aircraft.get_velocity_air_ef(); + const float speed = vel_air_ef.length(); + float drag_force = areaCd * 0.5 * air_density * sq(speed); + Vector3f drag_ef; + if (is_positive(speed)) { + drag_ef = -(vel_air_ef / speed) * drag_force; + } + body_accel += aircraft.get_dcm().transposed() * drag_ef / mass; } + float voltage_drop = current * model.refBatRes; + voltage_filter.apply(AP::sitl()->batt_voltage - voltage_drop); + // add some noise const float gyro_noise = radians(0.1); const float accel_noise = 0.3; - const float noise_scale = thrust.length() / (thrust_scale * num_motors); + const float noise_scale = thrust.length() / thrust_max; rot_accel += Vector3f(aircraft.rand_normal(0, 1), aircraft.rand_normal(0, 1), aircraft.rand_normal(0, 1)) * gyro_noise * noise_scale; @@ -359,16 +533,11 @@ void Frame::calculate_forces(const Aircraft &aircraft, // calculate current and voltage -void Frame::current_and_voltage(const struct sitl_input &input, float &voltage, float ¤t) +void Frame::current_and_voltage(float &voltage, float ¤t) { - voltage = 0; + voltage = voltage_filter.get(); current = 0; for (uint8_t i=0; i namespace SITL { @@ -44,19 +45,92 @@ public: static Frame *find_frame(const char *name); // initialise frame - void init(float mass, float hover_throttle, float terminal_velocity, float terminal_rotation_rate); + void init(const char *frame_str); // calculate rotational and linear accelerations void calculate_forces(const Aircraft &aircraft, const struct sitl_input &input, - Vector3f &rot_accel, Vector3f &body_accel, float* rpm); + Vector3f &rot_accel, Vector3f &body_accel, float* rpm, + bool use_drag=true); float terminal_velocity; float terminal_rotation_rate; - float thrust_scale; uint8_t motor_offset; // calculate current and voltage - void current_and_voltage(const struct sitl_input &input, float &voltage, float ¤t); + void current_and_voltage(float &voltage, float ¤t); + + // get mass in kg + float get_mass(void) const { + return mass; + } + +private: + /* + parameters that define the multicopter model. Can be loaded from + a json file to give a custom model + */ + const struct Model { + // model mass kg + float mass = 3.0; + + // diameter of model + float diagonal_size = 0.35; + + /* + the ref values are for a test at fixed angle, used to estimate drag + */ + float refSpd = 15.08; // m/s + float refAngle = 45; // degrees + float refVoltage = 12.09; // Volts + float refCurrent = 29.3; // Amps + float refAlt = 593; // altitude AMSL + float refTempC = 25; // temperature C + float refBatRes = 0.01; // BAT.Res + + // full pack voltage + float maxVoltage = 4.2*3; + + // CTUN.ThO at bover at refAlt + float hoverThrOut = 0.39; + + // MOT_THST_EXPO + float propExpo = 0.65; + + // scaling factor for yaw response, deg/sec + float refRotRate = 120; + + // MOT params are from the reference test + // MOT_PWM_MIN + float pwmMin = 1000; + // MOT_PWM_MAX + float pwmMax = 2000; + // MOT_SPIN_MIN + float spin_min = 0.15; + // MOT_SPIN_MAX + float spin_max = 0.95; + + // maximum slew rate of motors + float slew_max = 150; + } default_model; + + struct Model model; + + // exposed area times coefficient of drag + float areaCd; + float mass; + float velocity_max; + float thrust_max; + float effective_prop_area; + + // 10Hz filter for battery voltage + LowPassFilterFloat voltage_filter{10}; + + // get air density in kg/m^3 + float get_air_density(float alt_amsl) const; + + // load frame parameters from a json model file + void load_frame_params(const char *model_json); + }; } diff --git a/libraries/SITL/SIM_Motor.cpp b/libraries/SITL/SIM_Motor.cpp index 31025d4028..83a2fbe431 100644 --- a/libraries/SITL/SIM_Motor.cpp +++ b/libraries/SITL/SIM_Motor.cpp @@ -23,26 +23,47 @@ using namespace SITL; // calculate rotational accel and thrust for a motor void Motor::calculate_forces(const struct sitl_input &input, - const float thrust_scale, uint8_t motor_offset, Vector3f &rot_accel, - Vector3f &thrust) + Vector3f &thrust, + const Vector3f &velocity_air_bf, + float air_density, + float velocity_max, + float effective_prop_area, + float voltage) { // fudge factors - const float arm_scale = radians(5000); const float yaw_scale = radians(400); - // get motor speed from 0 to 1 - float motor_speed = constrain_float((input.servos[motor_offset+servo]-1100)/900.0, 0, 1); + const float pwm = input.servos[motor_offset+servo]; + float command = pwm_to_command(pwm); + float voltage_scale = voltage / voltage_max; + + // apply slew limiter to command + uint64_t now_us = AP_HAL::micros64(); + if (last_calc_us != 0 && slew_max > 0) { + float dt = (now_us - last_calc_us)*1.0e-6; + float slew_max_change = slew_max * dt; + command = constrain_float(command, last_command-slew_max_change, last_command+slew_max_change); + } + last_calc_us = now_us; + last_command = command; // the yaw torque of the motor - Vector3f rotor_torque(0, 0, yaw_factor * motor_speed * yaw_scale); + Vector3f rotor_torque(0, 0, yaw_factor * command * yaw_scale * voltage_scale); + + // calculate velocity into prop, clipping at zero, assumes zero roll/pitch + float velocity_in = MAX(0, -velocity_air_bf.z); // get thrust for untilted motor - thrust = {0, 0, -motor_speed}; + float motor_thrust = calc_thrust(command * voltage_scale, air_density, effective_prop_area, velocity_in, velocity_max); + + // thrust in NED + thrust = {0, 0, -motor_thrust}; // define the arm position relative to center of mass - Vector3f arm(arm_scale * cosf(radians(angle)), arm_scale * sinf(radians(angle)), 0); + Vector3f arm(cosf(radians(angle)), sinf(radians(angle)), 0); + arm *= diagonal_size; // work out roll and pitch of motor relative to it pointing straight up float roll = 0, pitch = 0; @@ -76,11 +97,17 @@ void Motor::calculate_forces(const struct sitl_input &input, rotor_torque = rotation * rotor_torque; } - // calculate total rotational acceleration - rot_accel = (arm % thrust) + rotor_torque; + // calculate torque in newton-meters + Vector3f torque = (arm % thrust) + rotor_torque; - // scale the thrust - thrust = thrust * thrust_scale; + // calculate total rotational acceleration + rot_accel.x = torque.x / moment_of_inertia.x; + rot_accel.y = torque.y / moment_of_inertia.y; + rot_accel.z = torque.z / moment_of_inertia.z; + + // calculate current + float power = power_factor * fabsf(motor_thrust); + current = power / MAX(voltage, 0.1); } /* @@ -111,17 +138,56 @@ uint16_t Motor::update_servo(uint16_t demand, uint64_t time_usec, float &last_va // calculate current and voltage -void Motor::current_and_voltage(const struct sitl_input &input, float &voltage, float ¤t, - uint8_t motor_offset) +float Motor::get_current(void) const { - // get motor speed from 0 to 1 - float motor_speed = constrain_float((input.servos[motor_offset+servo]-1100)/900.0, 0, 1); - - // assume 10A per motor at full speed - current = 10 * motor_speed; - - // assume 3S, and full throttle drops voltage by 0.7V - if (AP::sitl()) { - voltage = AP::sitl()->batt_voltage - motor_speed * 0.7; - } + return current; +} + +// setup PWM ranges for this motor +void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max, + float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max) +{ + mot_pwm_min = _pwm_min; + mot_pwm_max = _pwm_max; + mot_spin_min = _spin_min; + mot_spin_max = _spin_max; + mot_expo = _expo; + slew_max = _slew_max; + vehicle_mass = _vehicle_mass; + diagonal_size = _diagonal_size; + power_factor = _power_factor; + voltage_max = _voltage_max; + + // assume 50% of mass on ring around center + moment_of_inertia.x = vehicle_mass * 0.25 * sq(diagonal_size*0.5); + moment_of_inertia.y = moment_of_inertia.x; + moment_of_inertia.z = vehicle_mass * 0.5 * sq(diagonal_size*0.5); +} + +/* + convert a PWM value to a command value from 0 to 1 +*/ +float Motor::pwm_to_command(float pwm) const +{ + const float pwm_thrust_max = mot_pwm_min + mot_spin_max * (mot_pwm_max - mot_pwm_min); + const float pwm_thrust_min = mot_pwm_min + mot_spin_min * (mot_pwm_max - mot_pwm_min); + const float pwm_thrust_range = pwm_thrust_max - pwm_thrust_min; + return constrain_float((pwm-pwm_thrust_min)/pwm_thrust_range, 0, 1); +} + +/* + calculate thrust given a command value +*/ +float Motor::calc_thrust(float command, float air_density, float effective_prop_area, float velocity_in, float velocity_max) const +{ + float velocity_out = velocity_max * sqrtf((1-mot_expo)*command + mot_expo*sq(command)); + float ret = 0.5 * air_density * effective_prop_area * (sq(velocity_out) - sq(velocity_in)); +#if 0 + if (command > 0) { + ::printf("air_density=%f effective_prop_area=%f velocity_in=%f velocity_max=%f\n", + air_density, effective_prop_area, velocity_in, velocity_max); + ::printf("calc_thrust %.3f %.3f\n", command, ret); + } +#endif + return ret; } diff --git a/libraries/SITL/SIM_Motor.h b/libraries/SITL/SIM_Motor.h index 47e629ee2f..5c5a6199ee 100644 --- a/libraries/SITL/SIM_Motor.h +++ b/libraries/SITL/SIM_Motor.h @@ -70,15 +70,51 @@ public: {} void calculate_forces(const struct sitl_input &input, - float thrust_scale, uint8_t motor_offset, Vector3f &rot_accel, // rad/sec - Vector3f &body_thrust); // Z is down + Vector3f &body_thrust, // Z is down + const Vector3f &velocity_air_bf, + float air_density, + float velocity_max, + float effective_prop_area, + float voltage); uint16_t update_servo(uint16_t demand, uint64_t time_usec, float &last_value); - // calculate current and voltage - void current_and_voltage(const struct sitl_input &input, float &voltage, float ¤t, uint8_t motor_offset); + // get current + float get_current(void) const; + + // convert a PWM value to a thrust demand from 0 to 1 + float pwm_to_command(float pwm) const; + + // setup motor key parameters + void setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max, + float _vehicle_mass, float _diagonal_size, float _power_factor, float _voltage_max); + + // override slew limit + void set_slew_max(float _slew_max) { + slew_max = _slew_max; + } + + // calculate thrust of motor + float calc_thrust(float command, float air_density, float effective_prop_area, float velocity_in, float velocity_max) const; + +private: + float mot_pwm_min; + float mot_pwm_max; + float mot_spin_min; + float mot_spin_max; + float mot_expo; + float slew_max; + float vehicle_mass; + float diagonal_size; + float current; + float power_factor; + float voltage_max; + Vector3f moment_of_inertia; + + float last_command; + uint64_t last_calc_us; }; } diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 2d51cabe35..25369398b0 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -26,20 +26,15 @@ using namespace SITL; MultiCopter::MultiCopter(const char *frame_str) : Aircraft(frame_str) { - mass = 1.5f; - frame = Frame::find_frame(frame_str); if (frame == nullptr) { printf("Frame '%s' not found", frame_str); exit(1); } - // initial mass is passed through to Frame for it to calculate a - // hover thrust requirement. - if (strstr(frame_str, "-fast")) { - frame->init(gross_mass(), 0.5, 85, 4*radians(360)); - } else { - frame->init(gross_mass(), 0.51, 15, 4*radians(360)); - } + + frame->init(frame_str); + + mass = frame->get_mass(); frame_height = 0.1; num_motors = frame->num_motors; ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT; @@ -67,7 +62,7 @@ void MultiCopter::update(const struct sitl_input &input) calculate_forces(input, rot_accel, accel_body); // estimate voltage and current - frame->current_and_voltage(input, battery_voltage, battery_current); + frame->current_and_voltage(battery_voltage, battery_current); update_dynamics(rot_accel); update_external_payload(input); diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index 9287a6fea5..15a97c3ee6 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -87,7 +87,7 @@ QuadPlane::QuadPlane(const char *frame_str) : frame->motor_offset = motor_offset; // we use zero terminal velocity to let the plane model handle the drag - frame->init(mass, 0.51, 0, 0); + frame->init(frame_str); ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT; } @@ -108,10 +108,10 @@ void QuadPlane::update(const struct sitl_input &input) Vector3f quad_rot_accel; Vector3f quad_accel_body; - frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, &rpm[1]); + frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, &rpm[1], false); // estimate voltage and current - frame->current_and_voltage(input, battery_voltage, battery_current); + frame->current_and_voltage(battery_voltage, battery_current); float throttle; if (reverse_thrust) {