From c807267babce084353f2bf3143050dc68a6158d6 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 27 Sep 2019 20:56:15 +0100 Subject: [PATCH] SITL: add rpm-based motor noise per-motor to gyros and accels convert multicopter thrust to rpm per motor --- libraries/SITL/SIM_Aircraft.cpp | 5 +++-- libraries/SITL/SIM_Aircraft.h | 4 ++-- libraries/SITL/SIM_EFI_MegaSquirt.cpp | 2 +- libraries/SITL/SIM_FlightAxis.cpp | 5 +++-- libraries/SITL/SIM_Frame.cpp | 13 ++++++++++--- libraries/SITL/SIM_Frame.h | 2 +- libraries/SITL/SIM_Helicopter.cpp | 2 +- libraries/SITL/SIM_JSBSim.cpp | 5 +++-- libraries/SITL/SIM_Multicopter.cpp | 5 ++++- libraries/SITL/SIM_Plane.cpp | 3 ++- libraries/SITL/SIM_QuadPlane.cpp | 3 ++- libraries/SITL/SIM_Sailboat.cpp | 2 +- libraries/SITL/SIM_Scrimmage.cpp | 4 ++-- libraries/SITL/SIM_XPlane.cpp | 5 +++-- libraries/SITL/SITL.cpp | 3 +++ libraries/SITL/SITL.h | 7 +++++-- 16 files changed, 46 insertions(+), 24 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 874f2d2ab0..669f19c17f 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -55,6 +55,7 @@ Aircraft::Aircraft(const char *frame_str) : rate_hz(1200.0f), autotest_dir(nullptr), frame(frame_str), + num_motors(1), #if defined(__CYGWIN__) || defined(__CYGWIN64__) min_sleep_time(20000) #else @@ -341,8 +342,8 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) fdm.airspeed = airspeed_pitot; fdm.battery_voltage = battery_voltage; fdm.battery_current = battery_current; - fdm.rpm1 = rpm1; - fdm.rpm2 = rpm2; + fdm.num_motors = num_motors; + memcpy(fdm.rpm, rpm, num_motors * sizeof(float)); fdm.rcin_chan_count = rcin_chan_count; fdm.range = range; memcpy(fdm.rcin, rcin, rcin_chan_count * sizeof(float)); diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 80a73719eb..768b3aaa79 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -155,8 +155,8 @@ protected: float airspeed_pitot; // m/s, apparent airspeed, as seen by fwd pitot tube float battery_voltage = -1.0f; float battery_current = 0.0f; - float rpm1 = 0; - float rpm2 = 0; + uint8_t num_motors = 1; + float rpm[12]; uint8_t rcin_chan_count = 0; float rcin[8]; float range = -1.0f; // rangefinder detection in m diff --git a/libraries/SITL/SIM_EFI_MegaSquirt.cpp b/libraries/SITL/SIM_EFI_MegaSquirt.cpp index 9520a90c67..1a9a376871 100644 --- a/libraries/SITL/SIM_EFI_MegaSquirt.cpp +++ b/libraries/SITL/SIM_EFI_MegaSquirt.cpp @@ -46,7 +46,7 @@ void EFI_MegaSquirt::update() if (!connected) { return; } - float rpm = sitl->state.rpm1; + float rpm = sitl->state.rpm[0]; table7.rpm = rpm; table7.fuelload = 20; diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index 5881ffaa1b..c16c8c7658 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -86,6 +86,7 @@ FlightAxis::FlightAxis(const char *frame_str) : Aircraft(frame_str) { use_time_sync = false; + num_motors = 2; rate_hz = 250 / target_speedup; heli_demix = strstr(frame_str, "helidemix") != nullptr; rev4_servos = strstr(frame_str, "rev4") != nullptr; @@ -473,8 +474,8 @@ void FlightAxis::update(const struct sitl_input &input) battery_voltage = state.m_batteryVoltage_VOLTS; battery_current = state.m_batteryCurrentDraw_AMPS; - rpm1 = state.m_heliMainRotorRPM; - rpm2 = state.m_propRPM; + rpm[0] = state.m_heliMainRotorRPM; + rpm[1] = state.m_propRPM; /* the interlink interface supports 8 input channels diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index d2da87ed67..5838c560e2 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -206,13 +206,13 @@ static Frame supported_frames[] = Frame("firefly", 6, firefly_motors) }; -void Frame::init(float _mass, float hover_throttle, float _terminal_velocity, float _terminal_rotation_rate) +void Frame::init(float _mass, float _hover_throttle, float _terminal_velocity, float _terminal_rotation_rate) { /* 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); + thrust_scale = (_mass * GRAVITY_MSS) / (num_motors * _hover_throttle); terminal_velocity = _terminal_velocity; terminal_rotation_rate = _terminal_rotation_rate; @@ -228,6 +228,8 @@ Frame *Frame::find_frame(const char *name) if (strncasecmp(name, supported_frames[i].name, strlen(supported_frames[i].name)) == 0) { return &supported_frames[i]; } + + } return nullptr; } @@ -236,7 +238,8 @@ Frame *Frame::find_frame(const char *name) void Frame::calculate_forces(const Aircraft &aircraft, const struct sitl_input &input, Vector3f &rot_accel, - Vector3f &body_accel) + Vector3f &body_accel, + float* rpm) { Vector3f thrust; // newtons @@ -245,6 +248,10 @@ void Frame::calculate_forces(const Aircraft &aircraft, motors[i].calculate_forces(input, thrust_scale, motor_offset, mraccel, mthrust); rot_accel += mraccel; thrust += mthrust; + // simulate motor rpm + if (!is_zero(AP::sitl()->vibe_motor)) { + rpm[i] = sqrtf(mthrust.length() / thrust_scale) * AP::sitl()->vibe_motor * 60.0f; + } } body_accel = thrust/aircraft.gross_mass(); diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h index f231df48e3..96db2e2b94 100644 --- a/libraries/SITL/SIM_Frame.h +++ b/libraries/SITL/SIM_Frame.h @@ -49,7 +49,7 @@ public: // calculate rotational and linear accelerations void calculate_forces(const Aircraft &aircraft, const struct sitl_input &input, - Vector3f &rot_accel, Vector3f &body_accel); + Vector3f &rot_accel, Vector3f &body_accel, float* rpm); float terminal_velocity; float terminal_rotation_rate; diff --git a/libraries/SITL/SIM_Helicopter.cpp b/libraries/SITL/SIM_Helicopter.cpp index d9c27f7e09..2a11d7cc1f 100644 --- a/libraries/SITL/SIM_Helicopter.cpp +++ b/libraries/SITL/SIM_Helicopter.cpp @@ -152,7 +152,7 @@ void Helicopter::update(const struct sitl_input &input) Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity); // simulate rotor speed - rpm1 = thrust * 1300; + rpm[0] = thrust * 1300; // scale thrust to newtons thrust *= thrust_scale; diff --git a/libraries/SITL/SIM_JSBSim.cpp b/libraries/SITL/SIM_JSBSim.cpp index 94e8eb9274..6528580a57 100644 --- a/libraries/SITL/SIM_JSBSim.cpp +++ b/libraries/SITL/SIM_JSBSim.cpp @@ -62,6 +62,7 @@ JSBSim::JSBSim(const char *frame_str) : } control_port = 5505 + instance*10; fdm_port = 5504 + instance*10; + num_motors = 2; printf("JSBSim backend started: control_port=%u fdm_port=%u\n", control_port, fdm_port); @@ -443,8 +444,8 @@ void JSBSim::recv_fdm(const struct sitl_input &input) // update magnetic field update_mag_field_bf(); - rpm1 = fdm.rpm[0]; - rpm2 = fdm.rpm[1]; + rpm[0] = fdm.rpm[0]; + rpm[1] = fdm.rpm[1]; time_now_us = fdm.cur_time; } diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index e568d9405e..d45085fe49 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -42,13 +42,15 @@ MultiCopter::MultiCopter(const char *frame_str) : frame->init(gross_mass(), 0.51, 15, 4*radians(360)); } frame_height = 0.1; + num_motors = frame->num_motors; ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT; } // calculate rotational and linear accelerations void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel) { - frame->calculate_forces(*this, input, rot_accel, body_accel); + frame->calculate_forces(*this, input, rot_accel, body_accel, rpm); + add_shove_forces(rot_accel, body_accel); add_twist_forces(rot_accel); } @@ -78,3 +80,4 @@ void MultiCopter::update(const struct sitl_input &input) // update magnetic field update_mag_field_bf(); } + diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index 36b532c57a..494b4cb8f5 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/libraries/SITL/SIM_Plane.cpp @@ -34,6 +34,7 @@ Plane::Plane(const char *frame_str) : */ thrust_scale = (mass * GRAVITY_MSS) / hover_throttle; frame_height = 0.1f; + num_motors = 1; ground_behavior = GROUND_BEHAVIOR_FWD_ONLY; @@ -342,7 +343,7 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel } // simulate engine RPM - rpm1 = thrust * 7000; + rpm[0] = thrust * 7000; // scale thrust to newtons thrust *= thrust_scale; diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index 3c8aee544d..ee91247bb0 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -73,6 +73,7 @@ QuadPlane::QuadPlane(const char *frame_str) : printf("Failed to find frame '%s'\n", frame_type); exit(1); } + num_motors = 1 + frame->num_motors; if (strstr(frame_str, "cl84")) { // setup retract servos at front @@ -107,7 +108,7 @@ 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); + frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, &rpm[1]); // estimate voltage and current frame->current_and_voltage(input, battery_voltage, battery_current); diff --git a/libraries/SITL/SIM_Sailboat.cpp b/libraries/SITL/SIM_Sailboat.cpp index 6a81989a9c..658934c4c0 100644 --- a/libraries/SITL/SIM_Sailboat.cpp +++ b/libraries/SITL/SIM_Sailboat.cpp @@ -182,7 +182,7 @@ void Sailboat::update(const struct sitl_input &input) const float wind_apparent_dir_bf = wrap_180(wind_apparent_dir_ef - degrees(AP::ahrs().yaw)); // set RPM and airspeed from wind speed, allows to test RPM and Airspeed wind vane back end in SITL - rpm1 = wind_apparent_speed; + rpm[0] = wind_apparent_speed; airspeed_pitot = wind_apparent_speed; // calculate angle-of-attack from wind to mainsail diff --git a/libraries/SITL/SIM_Scrimmage.cpp b/libraries/SITL/SIM_Scrimmage.cpp index d0f1e57633..2901198d2c 100644 --- a/libraries/SITL/SIM_Scrimmage.cpp +++ b/libraries/SITL/SIM_Scrimmage.cpp @@ -190,8 +190,8 @@ void Scrimmage::recv_fdm(const struct sitl_input &input) battery_voltage = 0; battery_current = 0; - rpm1 = 0; - rpm2 = 0; + rpm[0] = 0; + rpm[1] = 0; airspeed = pkt.airspeed; airspeed_pitot = pkt.airspeed; diff --git a/libraries/SITL/SIM_XPlane.cpp b/libraries/SITL/SIM_XPlane.cpp index d4c58c1587..87cc6be037 100644 --- a/libraries/SITL/SIM_XPlane.cpp +++ b/libraries/SITL/SIM_XPlane.cpp @@ -42,6 +42,7 @@ XPlane::XPlane(const char *frame_str) : } heli_frame = (strstr(frame_str, "-heli") != nullptr); + num_motors = 2; socket_in.bind("0.0.0.0", bind_port); printf("Waiting for XPlane data on UDP port %u and sending to port %u\n", @@ -258,11 +259,11 @@ bool XPlane::receive_data(void) } case EngineRPM: - rpm1 = data[1]; + rpm[0] = data[1]; break; case PropRPM: - rpm2 = data[1]; + rpm[1] = data[1]; break; case Joystick2: diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index ff21c9b43c..08c989de06 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -204,6 +204,9 @@ const AP_Param::GroupInfo SITL::var_info2[] = { AP_GROUPINFO("MAG_SCALING", 60, SITL, mag_scaling, 1), + // max motor vibration frequency + AP_GROUPINFO("VIB_MOT_MAX", 61, SITL, vibe_motor, 0.0f), + AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 5b444769ba..a468e8a93e 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -45,8 +45,8 @@ struct sitl_fdm { double airspeed; // m/s double battery_voltage; // Volts double battery_current; // Amps - double rpm1; // main prop RPM - double rpm2; // secondary RPM + uint8_t num_motors; + float rpm[12]; // RPM of all motors uint8_t rcin_chan_count; float rcin[8]; // RC input 0..1 double range; // rangefinder value @@ -248,6 +248,9 @@ public: // vibration frequencies in Hz on each axis AP_Vector3f vibe_freq; + // hover frequency to use as baseline for adding motor noise for the gyros and accels + AP_Float vibe_motor; + // gyro and accel fail masks AP_Int8 gyro_fail_mask; AP_Int8 accel_fail_mask;