From a0af08a3aa188a618cf8bc7d441030212a7a03a2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 18 Aug 2022 17:18:32 +1000 Subject: [PATCH] SITL switched to motor_mask for which actuators are motors this allows for any output to be an ESC, which allows for proper simulation of quadplanes with ESCs on outputs 5-8 or 9-12, for testing notch filtering --- libraries/SITL/SIM_Aircraft.cpp | 5 ++--- libraries/SITL/SIM_Aircraft.h | 15 ++------------- libraries/SITL/SIM_FlightAxis.cpp | 1 - libraries/SITL/SIM_Frame.cpp | 2 +- libraries/SITL/SIM_JSBSim.cpp | 1 - libraries/SITL/SIM_Multicopter.cpp | 2 +- libraries/SITL/SIM_Multicopter.h | 5 ----- libraries/SITL/SIM_Plane.cpp | 4 ++-- libraries/SITL/SIM_QuadPlane.cpp | 5 ++--- libraries/SITL/SIM_QuadPlane.h | 6 ------ libraries/SITL/SIM_XPlane.cpp | 1 - libraries/SITL/SITL.h | 4 ++-- 12 files changed, 12 insertions(+), 39 deletions(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 4b7c0fcac5..13828d49f5 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -390,9 +390,8 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) fdm.velocity_air_bf = velocity_air_bf; fdm.battery_voltage = battery_voltage; fdm.battery_current = battery_current; - fdm.num_motors = num_motors; - fdm.vtol_motor_start = vtol_motor_start; - memcpy(fdm.rpm, rpm, num_motors * sizeof(float)); + fdm.motor_mask = motor_mask; + memcpy(fdm.rpm, rpm, sizeof(fdm.rpm)); fdm.rcin_chan_count = rcin_chan_count; fdm.range = rangefinder_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 1af7500e9a..a90246488c 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -91,16 +91,6 @@ public: // get frame rate of model in Hz float get_rate_hz(void) const { return rate_hz; } - // get number of motors for model - uint16_t get_num_motors() const { - return num_motors; - } - - // get motor offset for model - virtual uint16_t get_motors_offset() const { - return 0; - } - const Vector3f &get_gyro(void) const { return gyro; } @@ -189,9 +179,8 @@ protected: // battery model Battery battery; - uint8_t num_motors = 1; - uint8_t vtol_motor_start; - float rpm[12]; + uint32_t motor_mask; + float rpm[32]; uint8_t rcin_chan_count; float rcin[12]; diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index 811cecb2b5..0355b2ede9 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -109,7 +109,6 @@ 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; diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index 7bd65141e5..ea7c96ba76 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -589,7 +589,7 @@ void Frame::calculate_forces(const Aircraft &aircraft, thrust += mthrust; // simulate motor rpm if (!is_zero(AP::sitl()->vibe_motor)) { - rpm[i] = motors[i].get_command() * AP::sitl()->vibe_motor * 60.0f; + rpm[motor_offset+i] = motors[i].get_command() * AP::sitl()->vibe_motor * 60.0f; } } diff --git a/libraries/SITL/SIM_JSBSim.cpp b/libraries/SITL/SIM_JSBSim.cpp index ea798601db..1e1cc7539f 100644 --- a/libraries/SITL/SIM_JSBSim.cpp +++ b/libraries/SITL/SIM_JSBSim.cpp @@ -64,7 +64,6 @@ 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); diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index e271849587..a834cc6f85 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -36,7 +36,6 @@ MultiCopter::MultiCopter(const char *frame_str) : mass = frame->get_mass(); frame_height = 0.1; - num_motors = frame->num_motors; ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT; lock_step_scheduled = true; } @@ -44,6 +43,7 @@ MultiCopter::MultiCopter(const char *frame_str) : // calculate rotational and linear accelerations void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel) { + motor_mask |= ((1U<num_motors)-1U) << frame->motor_offset; frame->calculate_forces(*this, input, rot_accel, body_accel, rpm); add_shove_forces(rot_accel, body_accel); diff --git a/libraries/SITL/SIM_Multicopter.h b/libraries/SITL/SIM_Multicopter.h index bac5b6a80a..fa9a869358 100644 --- a/libraries/SITL/SIM_Multicopter.h +++ b/libraries/SITL/SIM_Multicopter.h @@ -34,11 +34,6 @@ public: /* update model by one time step */ void update(const struct sitl_input &input) override; - // get motor offset for model - virtual uint16_t get_motors_offset() const override { - return frame->motor_offset; - } - /* static object creator */ static Aircraft *create(const char *frame_str) { return new MultiCopter(frame_str); diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index f7b61f7df3..9ee8e073b9 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/libraries/SITL/SIM_Plane.cpp @@ -36,7 +36,6 @@ 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; lock_step_scheduled = true; @@ -354,7 +353,8 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel } // simulate engine RPM - rpm[0] = thrust * 7000; + motor_mask |= (1U<<2); + rpm[2] = thrust * 7000; // scale thrust to newtons thrust *= thrust_scale; diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index 37468a9b20..a58b673405 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -80,8 +80,6 @@ QuadPlane::QuadPlane(const char *frame_str) : printf("Failed to find frame '%s'\n", frame_type); exit(1); } - num_motors = 1 + frame->num_motors; - vtol_motor_start = 1; if (strstr(frame_str, "cl84")) { // setup retract servos at front @@ -120,7 +118,8 @@ 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], false); + motor_mask |= ((1U<num_motors)-1U) << frame->motor_offset; + frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, rpm, false); // rotate frames for copter tailsitters if (copter_tailsitter) { diff --git a/libraries/SITL/SIM_QuadPlane.h b/libraries/SITL/SIM_QuadPlane.h index 9df0212d4a..f111b4040c 100644 --- a/libraries/SITL/SIM_QuadPlane.h +++ b/libraries/SITL/SIM_QuadPlane.h @@ -39,12 +39,6 @@ public: return new QuadPlane(frame_str); } - // get motor offset for model - virtual uint16_t get_motors_offset() const override { - return frame->motor_offset; - } - - private: Frame *frame; }; diff --git a/libraries/SITL/SIM_XPlane.cpp b/libraries/SITL/SIM_XPlane.cpp index dbac7d2ff5..a8a425bb15 100644 --- a/libraries/SITL/SIM_XPlane.cpp +++ b/libraries/SITL/SIM_XPlane.cpp @@ -47,7 +47,6 @@ 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", diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index b5b9951e9d..cacc89875b 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -63,8 +63,8 @@ struct sitl_fdm { double battery_current; // Amps double battery_remaining; // Ah, if non-zero capacity uint8_t num_motors; - uint8_t vtol_motor_start; - float rpm[12]; // RPM of all motors + uint32_t motor_mask; + float rpm[32]; // RPM of all motors uint8_t rcin_chan_count; float rcin[12]; // RC input 0..1 double range; // rangefinder value