From 37f7cc4bcf900fc78abb91e1a5e32637b77172e7 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Tue, 31 Jul 2018 22:33:23 +1000 Subject: [PATCH] SITL: rearrange Grippers and Sprayers in SITL --- libraries/SITL/SIM_Aircraft.cpp | 21 ++++++- libraries/SITL/SIM_Aircraft.h | 67 +++++++++++----------- libraries/SITL/SIM_Calibration.cpp | 8 +-- libraries/SITL/SIM_Calibration.h | 8 +-- libraries/SITL/SIM_Frame.cpp | 2 +- libraries/SITL/SIM_Frame.h | 2 +- libraries/SITL/SIM_Gripper_EPM.cpp | 62 +++++++++++++------- libraries/SITL/SIM_Gripper_EPM.h | 24 +++++--- libraries/SITL/SIM_Gripper_Servo.cpp | 49 ++++++++++++---- libraries/SITL/SIM_Gripper_Servo.h | 26 ++++----- libraries/SITL/SIM_ICEngine.cpp | 2 +- libraries/SITL/SIM_ICEngine.h | 2 +- libraries/SITL/SIM_Motor.cpp | 2 +- libraries/SITL/SIM_Motor.h | 2 +- libraries/SITL/SIM_Multicopter.cpp | 15 +---- libraries/SITL/SIM_Multicopter.h | 12 ---- libraries/SITL/SIM_Sprayer.cpp | 85 +++++++++++++++++++--------- libraries/SITL/SIM_Sprayer.h | 35 +++++++----- libraries/SITL/SITL.cpp | 10 ++++ libraries/SITL/SITL.h | 11 +++- libraries/SITL/SITL_Input.h | 18 ++++++ 21 files changed, 295 insertions(+), 168 deletions(-) create mode 100644 libraries/SITL/SITL_Input.h diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 4792ee4cbb..1298ef1b9e 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -84,7 +84,6 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) : // allow for orientation settings, such as with tailsitters enum ap_var_type ptype; ahrs_orientation = (AP_Int8 *)AP_Param::find("AHRS_ORIENTATION", &ptype); - terrain = reinterpret_cast(AP_Param::find_object("TERRAIN_")); } @@ -737,4 +736,24 @@ void Aircraft::extrapolate_sensors(float delta_time) velocity_air_bf = dcm.transposed() * velocity_air_ef; } +void Aircraft::update_external_payload(const struct sitl_input &input) +{ + external_payload_mass = 0; + // update sprayer + if (sprayer && sprayer->is_enabled()) { + sprayer->update(input); + external_payload_mass += sprayer->payload_mass(); + } + + // update grippers + if (gripper && gripper->is_enabled()) { + gripper->set_alt(hagl()); + gripper->update(input); + external_payload_mass += gripper->payload_mass(); + } + if (gripper_epm && gripper_epm->is_enabled()) { + gripper_epm->update(input); + external_payload_mass += gripper_epm->payload_mass(); + } +} diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index b5f4f8f99f..8e51486708 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -21,8 +21,11 @@ #include #include "SITL.h" +#include "SITL_Input.h" #include - +#include "SIM_Sprayer.h" +#include "SIM_Gripper_Servo.h" +#include "SIM_Gripper_EPM.h" namespace SITL { @@ -30,25 +33,9 @@ namespace SITL { parent class for all simulator types */ class Aircraft { - friend class Gripper_Servo; - public: Aircraft(const char *home_str, const char *frame_str); - /* - structure passed in giving servo positions as PWM values in - microseconds - */ - struct sitl_input { - uint16_t servos[16]; - struct { - float speed; // m/s - float direction; // degrees 0..360 - float turbulence; - float dir_z; //degrees -90..90 - } wind; - }; - /* set simulation speedup */ @@ -111,7 +98,7 @@ public: return mag_bf; } - virtual float gross_mass() const { return mass; } + float gross_mass() const { return mass + external_payload_mass; } const Location &get_location() const { return location; } @@ -121,6 +108,10 @@ public: attitude.from_rotation_matrix(dcm); } + void set_sprayer(Sprayer *_sprayer) { sprayer = _sprayer; } + void set_gripper_servo(Gripper_Servo *_gripper) { gripper = _gripper; } + void set_gripper_epm(Gripper_EPM *_gripper_epm) { gripper_epm = _gripper_epm; } + protected: SITL *sitl; Location home; @@ -129,26 +120,27 @@ protected: float ground_level; float home_yaw; float frame_height; - Matrix3f dcm; // rotation matrix, APM conventions, from body to earth - Vector3f gyro; // rad/s - Vector3f gyro_prev; // rad/s - Vector3f ang_accel; // rad/s/s - Vector3f velocity_ef; // m/s, earth frame - Vector3f wind_ef; // m/s, earth frame - Vector3f velocity_air_ef; // velocity relative to airmass, earth frame - Vector3f velocity_air_bf; // velocity relative to airmass, body frame - Vector3f position; // meters, NED from origin - float mass; // kg - Vector3f accel_body; // m/s/s NED, body frame - float airspeed; // m/s, apparent airspeed - float airspeed_pitot; // m/s, apparent airspeed, as seen by fwd pitot tube + Matrix3f dcm; // rotation matrix, APM conventions, from body to earth + Vector3f gyro; // rad/s + Vector3f gyro_prev; // rad/s + Vector3f ang_accel; // rad/s/s + Vector3f velocity_ef; // m/s, earth frame + Vector3f wind_ef; // m/s, earth frame + Vector3f velocity_air_ef; // velocity relative to airmass, earth frame + Vector3f velocity_air_bf; // velocity relative to airmass, body frame + Vector3f position; // meters, NED from origin + float mass; // kg + float external_payload_mass = 0.0f; // kg + Vector3f accel_body; // m/s/s NED, body frame + float airspeed; // m/s, apparent airspeed + 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 rcin_chan_count = 0; float rcin[8]; - float range = -1.0f; // rangefinder detection in m + float range = -1.0f; // rangefinder detection in m // Wind Turbulence simulated Data float turbulence_azimuth = 0.0f; @@ -175,7 +167,7 @@ protected: // allow for AHRS_ORIENTATION AP_Int8 *ahrs_orientation; - + enum { GROUND_BEHAVIOR_NONE = 0, GROUND_BEHAVIOR_NO_MOVEMENT, @@ -234,7 +226,10 @@ protected: // extrapolate sensors by a given delta time in seconds void extrapolate_sensors(float delta_time); - + + // update external payload/sensor dynamic + void update_external_payload(const struct sitl_input &input); + private: uint64_t last_time_us = 0; uint32_t frame_counter = 0; @@ -253,6 +248,10 @@ private: } smoothing; LowPassFilterFloat servo_filter[4]; + + Sprayer *sprayer; + Gripper_Servo *gripper; + Gripper_EPM *gripper_epm; }; } // namespace SITL diff --git a/libraries/SITL/SIM_Calibration.cpp b/libraries/SITL/SIM_Calibration.cpp index 119f8eccf3..7160e5685d 100644 --- a/libraries/SITL/SIM_Calibration.cpp +++ b/libraries/SITL/SIM_Calibration.cpp @@ -30,7 +30,7 @@ SITL::Calibration::Calibration(const char *home_str, const char *frame_str) mass = 1.5f; } -void SITL::Calibration::update(const struct sitl_input& input) +void SITL::Calibration::update(const struct sitl_input &input) { Vector3f rot_accel{0, 0, 0}; @@ -54,7 +54,7 @@ void SITL::Calibration::update(const struct sitl_input& input) update_mag_field_bf(); } -void SITL::Calibration::_stop_control(const struct sitl_input& input, +void SITL::Calibration::_stop_control(const struct sitl_input &input, Vector3f& rot_accel) { Vector3f desired_angvel{0, 0, 0}; @@ -65,7 +65,7 @@ void SITL::Calibration::_stop_control(const struct sitl_input& input, rot_accel *= 0.002f; } -void SITL::Calibration::_attitude_control(const struct sitl_input& input, +void SITL::Calibration::_attitude_control(const struct sitl_input &input, Vector3f& rot_accel) { float desired_roll = -M_PI + 2 * M_PI * (input.servos[5] - 1000) / 1000.f; @@ -103,7 +103,7 @@ void SITL::Calibration::_attitude_set(float desired_roll, float desired_pitch, f rot_accel = error * (1.0f / dt); } -void SITL::Calibration::_angular_velocity_control(const struct sitl_input& in, +void SITL::Calibration::_angular_velocity_control(const struct sitl_input &in, Vector3f& rot_accel) { Vector3f axis{(float)(in.servos[5] - 1500), diff --git a/libraries/SITL/SIM_Calibration.h b/libraries/SITL/SIM_Calibration.h index 852a0e37b0..ab4411176b 100644 --- a/libraries/SITL/SIM_Calibration.h +++ b/libraries/SITL/SIM_Calibration.h @@ -66,22 +66,22 @@ class Calibration : public Aircraft { public: Calibration(const char *home_str, const char *frame_str); - void update(const struct sitl_input& input); + void update(const struct sitl_input &input); static Aircraft *create(const char *home_str, const char *frame_str) { return new Calibration(home_str, frame_str); } private: - void _stop_control(const struct sitl_input& input, Vector3f& rot_accel); + void _stop_control(const struct sitl_input &input, Vector3f& rot_accel); void _attitude_set(float desired_roll, float desired_pitch, float desired_yaw, Vector3f& rot_accel); - void _attitude_control(const struct sitl_input& input, + void _attitude_control(const struct sitl_input &input, Vector3f& rot_accel); - void _angular_velocity_control(const struct sitl_input& input, + void _angular_velocity_control(const struct sitl_input &input, Vector3f& rot_accel); void _calibration_poses(Vector3f& rot_accel); diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index 4034f3c7a5..083544d14c 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -202,7 +202,7 @@ Frame *Frame::find_frame(const char *name) // calculate rotational and linear accelerations void Frame::calculate_forces(const Aircraft &aircraft, - const Aircraft::sitl_input &input, + const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel) { diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h index 5695633075..be1b620e8f 100644 --- a/libraries/SITL/SIM_Frame.h +++ b/libraries/SITL/SIM_Frame.h @@ -48,7 +48,7 @@ public: // calculate rotational and linear accelerations void calculate_forces(const Aircraft &aircraft, - const Aircraft::sitl_input &input, + const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel); float terminal_velocity; diff --git a/libraries/SITL/SIM_Gripper_EPM.cpp b/libraries/SITL/SIM_Gripper_EPM.cpp index 2adb28c55a..0f9872288a 100644 --- a/libraries/SITL/SIM_Gripper_EPM.cpp +++ b/libraries/SITL/SIM_Gripper_EPM.cpp @@ -17,42 +17,65 @@ */ #include "SIM_Gripper_EPM.h" +#include "AP_HAL/AP_HAL.h" #include -#include +#include using namespace SITL; +// table of user settable parameters +const AP_Param::GroupInfo Gripper_EPM::var_info[] = { + + // @Param: ENABLE + // @DisplayName: Gripper servo Sim enable/disable + // @Description: Allows you to enable (1) or disable (0) the gripper servo simulation + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("ENABLE", 0, Gripper_EPM, gripper_emp_enable, 0), + + // @Param: PIN + // @DisplayName: Gripper emp pin + // @Description: The pin number that the gripper emp is connected to. (start at 1) + // @Range: 0 15 + // @User: Advanced + AP_GROUPINFO("PIN", 1, Gripper_EPM, gripper_emp_servo_pin, -1), + + AP_GROUPEND +}; + /* update gripper state */ -void Gripper_EPM::update_servobased(const Aircraft::sitl_input &input) +void Gripper_EPM::update_servobased(int16_t gripper_pwm) { - if (! servo_based) { + if (!servo_based) { return; } - demand = (input.servos[gripper_servo]-1000) * 0.001f; // 0.0 - 1.0 - if (demand < 0) { // never updated - demand = 0; + if (gripper_pwm >= 0) { + demand = (gripper_pwm - 1000) * 0.001f; // 0.0 - 1.0 + if (is_negative(demand)) { // never updated + demand = 0.0f; + } } } -void Gripper_EPM::update_from_demand(const Aircraft::sitl_input &input) +void Gripper_EPM::update_from_demand() { const uint64_t now = AP_HAL::micros64(); const float dt = (now - last_update_us) * 1.0e-6f; // decay the field - field_strength = field_strength * (100-field_decay_rate * dt)/100.0f; + field_strength = field_strength * (100.0f - field_decay_rate * dt) / 100.0f; // note that "demand" here is just an on/off switch; we only care // about which range it falls into - if (demand > 0.6) { + if (demand > 0.6f) { // we are instructed to grip harder - field_strength = field_strength + (100.0f-field_strength) * field_strength_slew_rate/100.0f * dt; - } else if (demand < 0.4) { + field_strength = field_strength + (100.0f - field_strength) * field_strength_slew_rate / 100.0f * dt; + } else if (demand < 0.4f) { // we are instructed to loosen grip - field_strength = field_strength * (100-field_degauss_rate * dt)/100.0f; + field_strength = field_strength * (100.0f - field_degauss_rate * dt) / 100.0f; } else { // neutral; no demanded change } @@ -66,14 +89,15 @@ void Gripper_EPM::update_from_demand(const Aircraft::sitl_input &input) } last_update_us = now; - return; } -void Gripper_EPM::update(const Aircraft::sitl_input &input) +void Gripper_EPM::update(const struct sitl_input &input) { - update_servobased(input); + const int16_t gripper_pwm = gripper_emp_servo_pin >= 1 ? input.servos[gripper_emp_servo_pin-1] : -1; - update_from_demand(input); + update_servobased(gripper_pwm); + + update_from_demand(); } @@ -83,7 +107,7 @@ bool Gripper_EPM::should_report() return false; } - if (fabs(reported_field_strength - field_strength) > 10.0f) { + if (abs(reported_field_strength - field_strength) > 10.0) { return true; } @@ -94,6 +118,6 @@ float Gripper_EPM::tesla() { // https://en.wikipedia.org/wiki/Orders_of_magnitude_(magnetic_field) // 200N lifting capacity ~= 2.5T - const float percentage_to_tesla = 0.25; - return percentage_to_tesla * field_strength/100.0f; + const float percentage_to_tesla = 0.25f; + return static_cast(percentage_to_tesla * field_strength / 100.0f); } diff --git a/libraries/SITL/SIM_Gripper_EPM.h b/libraries/SITL/SIM_Gripper_EPM.h index 028cc1dae5..e019f9f53f 100644 --- a/libraries/SITL/SIM_Gripper_EPM.h +++ b/libraries/SITL/SIM_Gripper_EPM.h @@ -18,23 +18,31 @@ #pragma once -#include "SIM_Aircraft.h" +#include "stdint.h" +#include +#include "SITL_Input.h" namespace SITL { class Gripper_EPM { public: - const uint8_t gripper_servo; + Gripper_EPM() { + AP_Param::setup_object_defaults(this, var_info); + }; - Gripper_EPM(const uint8_t _gripper_servo) : - gripper_servo(_gripper_servo) - {} + float payload_mass() const { return 0.0f; } // kg // update field stength - void update(const struct Aircraft::sitl_input &input); + void update(const struct sitl_input &input); + + static const struct AP_Param::GroupInfo var_info[]; + bool is_enabled() const {return static_cast(gripper_emp_enable);} private: + AP_Int8 gripper_emp_enable; // enable gripper sim + AP_Int8 gripper_emp_servo_pin; + const uint32_t report_interval = 100000; // microseconds uint64_t last_report_us; @@ -52,8 +60,8 @@ private: bool should_report(); - void update_from_demand(const Aircraft::sitl_input &input); - void update_servobased(const struct Aircraft::sitl_input &input); + void update_from_demand(); + void update_servobased(int16_t gripper_pwm); float tesla(); diff --git a/libraries/SITL/SIM_Gripper_Servo.cpp b/libraries/SITL/SIM_Gripper_Servo.cpp index d1c93c1631..5e4ec6ec36 100644 --- a/libraries/SITL/SIM_Gripper_Servo.cpp +++ b/libraries/SITL/SIM_Gripper_Servo.cpp @@ -17,29 +17,57 @@ */ #include "SIM_Gripper_Servo.h" +#include "AP_HAL/AP_HAL.h" +#include "AP_Math/AP_Math.h" #include using namespace SITL; +// table of user settable parameters +const AP_Param::GroupInfo Gripper_Servo::var_info[] = { + + // @Param: ENABLE + // @DisplayName: Gripper servo Sim enable/disable + // @Description: Allows you to enable (1) or disable (0) the gripper servo simulation + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("ENABLE", 0, Gripper_Servo, gripper_enable, 0), + + // @Param: PIN + // @DisplayName: Gripper servo pin + // @Description: The pin number that the gripper servo is connected to. (start at 1) + // @Range: 0 15 + // @User: Advanced + AP_GROUPINFO("PIN", 1, Gripper_Servo, gripper_servo_pin, -1), + + AP_GROUPEND +}; + /* update gripper state */ -void Gripper_Servo::update(const Aircraft::sitl_input &input) +void Gripper_Servo::update(const struct sitl_input &input) { + const int16_t gripper_pwm = gripper_servo_pin >= 1 ? input.servos[gripper_servo_pin-1] : -1; + const uint64_t now = AP_HAL::micros64(); const float dt = (now - last_update_us) * 1.0e-6f; // update gripper position - - float position_demand = (input.servos[gripper_servo]-1000) * 0.001f; - if (position_demand < 0) { // never updated - position_demand = 0; + if (gripper_pwm < 0) { + last_update_us = now; + return; } - const float position_max_change = position_slew_rate/100.0f * dt; - position = constrain_float(position_demand, position-position_max_change, position+position_max_change); + float position_demand = (gripper_pwm - 1000) * 0.001f; + if (is_negative(position_demand)) { // never updated + position_demand = 0.0f; + } - const float jaw_gap = gap*(1.0f-position); + const float position_max_change = position_slew_rate / 100.0f * dt; + position = constrain_float(position_demand, position - position_max_change, position + position_max_change); + + const float jaw_gap = gap * (1.0f - position); if (should_report()) { ::fprintf(stderr, "position_demand=%f jaw_gap=%f load=%f\n", position_demand, jaw_gap, load_mass); last_report_us = now; @@ -47,7 +75,7 @@ void Gripper_Servo::update(const Aircraft::sitl_input &input) } if (jaw_gap < 5) { - if (aircraft->on_ground()) { + if (altitude <= 0.0f) { load_mass = 1.0f; // attach the load } } else if (jaw_gap > 10) { @@ -55,7 +83,6 @@ void Gripper_Servo::update(const Aircraft::sitl_input &input) } last_update_us = now; - return; } bool Gripper_Servo::should_report() @@ -74,7 +101,7 @@ bool Gripper_Servo::should_report() float Gripper_Servo::payload_mass() const { - if (aircraft->hagl() < string_length) { + if (altitude < string_length) { return 0.0f; } return load_mass; diff --git a/libraries/SITL/SIM_Gripper_Servo.h b/libraries/SITL/SIM_Gripper_Servo.h index 5d991eb659..1e22a81359 100644 --- a/libraries/SITL/SIM_Gripper_Servo.h +++ b/libraries/SITL/SIM_Gripper_Servo.h @@ -17,35 +17,35 @@ */ #pragma once - -#include "SIM_Aircraft.h" +#include "stdint.h" +#include +#include "SITL_Input.h" namespace SITL { class Gripper_Servo { public: - const uint8_t gripper_servo; - - Gripper_Servo(const uint8_t _gripper_servo) : - gripper_servo(_gripper_servo) - {} + Gripper_Servo() { + AP_Param::setup_object_defaults(this, var_info); + }; // update Gripper state - void update(const struct Aircraft::sitl_input &input); + void update(const struct sitl_input &input); float payload_mass() const; // kg - void set_aircraft(Aircraft *_aircraft) { aircraft = _aircraft; } + void set_alt(float alt) {altitude = alt;}; + static const struct AP_Param::GroupInfo var_info[]; + bool is_enabled() const {return static_cast(gripper_enable);} private: - - Aircraft *aircraft; - + AP_Int8 gripper_enable; // enable gripper sim + AP_Int8 gripper_servo_pin; const uint32_t report_interval = 1000000; // microseconds uint64_t last_report_us; const float gap = 30; // mm - + float altitude; float position; // percentage float position_slew_rate = 35; // percentage float reported_position = -1; // unlikely diff --git a/libraries/SITL/SIM_ICEngine.cpp b/libraries/SITL/SIM_ICEngine.cpp index 0585ced374..2978c9542d 100644 --- a/libraries/SITL/SIM_ICEngine.cpp +++ b/libraries/SITL/SIM_ICEngine.cpp @@ -24,7 +24,7 @@ using namespace SITL; /* update engine state, returning power output from 0 to 1 */ -float ICEngine::update(const Aircraft::sitl_input &input) +float ICEngine::update(const struct sitl_input &input) { bool have_ignition = ignition_servo>=0; bool have_choke = choke_servo>=0; diff --git a/libraries/SITL/SIM_ICEngine.h b/libraries/SITL/SIM_ICEngine.h index 2b4d03013b..10423913e0 100644 --- a/libraries/SITL/SIM_ICEngine.h +++ b/libraries/SITL/SIM_ICEngine.h @@ -39,7 +39,7 @@ public: {} // update motor state - float update(const struct Aircraft::sitl_input &input); + float update(const struct sitl_input &input); private: float last_output; diff --git a/libraries/SITL/SIM_Motor.cpp b/libraries/SITL/SIM_Motor.cpp index c9326ba2f3..0f7ea8cf97 100644 --- a/libraries/SITL/SIM_Motor.cpp +++ b/libraries/SITL/SIM_Motor.cpp @@ -22,7 +22,7 @@ using namespace SITL; // calculate rotational accel and thrust for a motor -void Motor::calculate_forces(const Aircraft::sitl_input &input, +void Motor::calculate_forces(const struct sitl_input &input, const float thrust_scale, uint8_t motor_offset, Vector3f &rot_accel, diff --git a/libraries/SITL/SIM_Motor.h b/libraries/SITL/SIM_Motor.h index b263123f48..8bbf451a3a 100644 --- a/libraries/SITL/SIM_Motor.h +++ b/libraries/SITL/SIM_Motor.h @@ -69,7 +69,7 @@ public: pitch_max(_pitch_max) {} - void calculate_forces(const Aircraft::sitl_input &input, + void calculate_forces(const struct sitl_input &input, float thrust_scale, uint8_t motor_offset, Vector3f &rot_accel, // rad/sec diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 355e5b78df..875c8e08f4 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -29,8 +29,6 @@ MultiCopter::MultiCopter(const char *home_str, const char *frame_str) : { mass = 1.5f; - gripper.set_aircraft(this); - frame = Frame::find_frame(frame_str); if (frame == nullptr) { printf("Frame '%s' not found", frame_str); @@ -66,6 +64,7 @@ void MultiCopter::update(const struct sitl_input &input) calculate_forces(input, rot_accel, accel_body); update_dynamics(rot_accel); + update_external_payload(input); // update lat/lon/altitude update_position(); @@ -73,16 +72,4 @@ void MultiCopter::update(const struct sitl_input &input) // update magnetic field update_mag_field_bf(); - - // update sprayer - sprayer.update(input); - - // update gripper - gripper.update(input); - gripper_epm.update(input); -} - -float MultiCopter::gross_mass() const -{ - return Aircraft::gross_mass() + sprayer.payload_mass() + gripper.payload_mass(); } diff --git a/libraries/SITL/SIM_Multicopter.h b/libraries/SITL/SIM_Multicopter.h index b252dc2a04..2d9c88c6fb 100644 --- a/libraries/SITL/SIM_Multicopter.h +++ b/libraries/SITL/SIM_Multicopter.h @@ -22,10 +22,6 @@ #include "SIM_Motor.h" #include "SIM_Frame.h" -#include "SIM_Sprayer.h" -#include "SIM_Gripper_Servo.h" -#include "SIM_Gripper_EPM.h" - namespace SITL { /* @@ -47,14 +43,6 @@ protected: // calculate rotational and linear accelerations void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel); Frame *frame; - - // The numbers below are the pwm output channels with "0" meaning the first output (aka RC1) - Sprayer sprayer{6, 7}; - Gripper_Servo gripper{8}; - Gripper_EPM gripper_epm{9}; - - float gross_mass() const override; - }; } diff --git a/libraries/SITL/SIM_Sprayer.cpp b/libraries/SITL/SIM_Sprayer.cpp index 60093d5f24..1b4c549f6d 100644 --- a/libraries/SITL/SIM_Sprayer.cpp +++ b/libraries/SITL/SIM_Sprayer.cpp @@ -17,56 +17,89 @@ */ #include "SIM_Sprayer.h" -#include +#include "AP_HAL/AP_HAL.h" +#include "AP_Math/AP_Math.h" using namespace SITL; +// table of user settable parameters +const AP_Param::GroupInfo Sprayer::var_info[] = { + + // @Param: ENABLE + // @DisplayName: Gripper servo Sim enable/disable + // @Description: Allows you to enable (1) or disable (0) the gripper servo simulation + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("ENABLE", 0, Sprayer, sprayer_enable, 0), + + // @Param: PUMP + // @DisplayName: Sprayer pump pin + // @Description: The pin number that the Sprayer pump is connected to. (start at 1) + // @Range: 0 15 + // @User: Advanced + AP_GROUPINFO("PUMP", 1, Sprayer, sprayer_pump_pin, -1), + + // @Param: SPIN + // @DisplayName: Sprayer spinner servo pin + // @Description: The pin number that the Sprayer spinner servo is connected to. (start at 1) + // @Range: 0 15 + // @User: Advanced + AP_GROUPINFO("SPIN", 2, Sprayer, sprayer_spin_pin, -1), + + AP_GROUPEND +}; + /* update sprayer state */ -void Sprayer::update(const Aircraft::sitl_input &input) +void Sprayer::update(const struct sitl_input &input) { + const int16_t pump_pwm = sprayer_pump_pin >= 1 ? input.servos[sprayer_pump_pin-1] : -1; + const int16_t spinner_pwm = sprayer_spin_pin >= 1 ? input.servos[sprayer_spin_pin-1] : -1; const uint64_t now = AP_HAL::micros64(); const float dt = (now - last_update_us) * 1.0e-6f; - - // update remaining payload - if (capacity > 0) { - const double delta = last_pump_output * pump_max_rate * dt; - capacity -= delta; - if (capacity < 0) { - capacity = 0.0f; + if (pump_pwm >= 0) { + // update remaining payload + if (capacity > 0) { + const double delta = last_pump_output * pump_max_rate * dt; + capacity -= delta; + if (capacity < 0) { + capacity = 0.0f; + } } - } - // update pump - float pump_demand = (input.servos[pump_servo]-1000) * 0.001f; - // ::fprintf(stderr, "pump_demand=%f\n", pump_demand); - if (pump_demand < 0) { // never updated - pump_demand = 0; + // update pump + float pump_demand = (pump_pwm - 1000) * 0.001f; + // ::fprintf(stderr, "pump_demand=%f\n", pump_demand); + if (pump_demand < 0) { // never updated + pump_demand = 0; + } + const float pump_max_change = pump_slew_rate / 100.0f * dt; + last_pump_output = + constrain_float(pump_demand, last_pump_output - pump_max_change, last_pump_output + pump_max_change); + last_pump_output = constrain_float(last_pump_output, 0, 1); + } else { + last_pump_output = 0.0f; } - const float pump_max_change = pump_slew_rate/100.0f * dt; - last_pump_output = constrain_float(pump_demand, last_pump_output-pump_max_change, last_pump_output+pump_max_change); - last_pump_output = constrain_float(last_pump_output, 0, 1); - // update spinner (if any) - if (spinner_servo >= 0) { - const float spinner_demand = (input.servos[spinner_servo]-1000) * 0.001f; + if (spinner_pwm >= 0) { + const float spinner_demand = (spinner_pwm - 1000) * 0.001f; const float spinner_max_change = spinner_slew_rate * 0.01f * dt; - last_spinner_output = constrain_float(spinner_demand, last_spinner_output-spinner_max_change, last_spinner_output+spinner_max_change); + last_spinner_output = constrain_float(spinner_demand, + last_spinner_output - spinner_max_change, + last_spinner_output + spinner_max_change); last_spinner_output = constrain_float(last_spinner_output, 0, 1); } if (should_report()) { printf("Remaining: %f litres\n", capacity); printf("Pump: %f l/s\n", last_pump_output * pump_max_rate); - if (spinner_servo >= 0) { - printf("Spinner: %f rev/s\n", (last_spinner_output * spinner_max_rate)/360.0f); + if (spinner_pwm >= 0) { + printf("Spinner: %f rev/s\n", (last_spinner_output * spinner_max_rate) / 360.0f); } last_report_us = now; } - last_update_us = now; - return; } bool Sprayer::should_report() diff --git a/libraries/SITL/SIM_Sprayer.h b/libraries/SITL/SIM_Sprayer.h index 3db3ccc8a1..ae5f104b89 100644 --- a/libraries/SITL/SIM_Sprayer.h +++ b/libraries/SITL/SIM_Sprayer.h @@ -18,36 +18,41 @@ #pragma once -#include "SIM_Aircraft.h" +#include "stdint.h" +#include +#include "SITL_Input.h" namespace SITL { class Sprayer { public: - const uint8_t pump_servo; - const int8_t spinner_servo; - - Sprayer(const uint8_t _pump_servo, int8_t _spinner_servo) : - pump_servo(_pump_servo), - spinner_servo(_spinner_servo) - {} + Sprayer() { + AP_Param::setup_object_defaults(this, var_info); + }; // update sprayer state - void update(const struct Aircraft::sitl_input &input); + void update(const struct sitl_input &input); - float payload_mass() const { return capacity; }; // kg; water, so kg=l + float payload_mass() const { return static_cast(capacity); }; // kg; water, so kg=l -private: + static const struct AP_Param::GroupInfo var_info[]; + bool is_enabled() const {return static_cast(sprayer_enable);} + + private: + + AP_Int8 sprayer_enable; // enable sprayer sim + AP_Int8 sprayer_pump_pin; + AP_Int8 sprayer_spin_pin; const uint32_t report_interval = 1000000; // microseconds uint64_t last_report_us; - const float pump_max_rate = 0.01; // litres/second - const float pump_slew_rate = 20; // percent/scond + const float pump_max_rate = 0.01f; // litres/second + const float pump_slew_rate = 20.0f; // percent/second float last_pump_output; // percentage - const float spinner_max_rate = 3600; // degrees/second - const float spinner_slew_rate = 20; // percent/second + const float spinner_max_rate = 3600.0f; // degrees/second + const float spinner_slew_rate = 20.0f; // percent/second float last_spinner_output; // percentage double capacity = 0.25; // litres diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 9db401082f..2f821bf587 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -122,6 +122,16 @@ const AP_Param::GroupInfo SITL::var_info2[] = { AP_GROUPINFO("MAG_ODI", 19, SITL, mag_offdiag, 0), AP_GROUPINFO("MAG_ORIENT", 20, SITL, mag_orient, 0), AP_GROUPINFO("RC_CHANCOUNT",21, SITL, rc_chancount, 16), + // @Group: SPR_ + // @Path: ./SIM_Sprayer.cpp + AP_SUBGROUPINFO(sprayer_sim, "SPR_", 22, SITL, Sprayer), + // @Group: GRPS_ + // @Path: ./SIM_Gripper_Servo.cpp + AP_SUBGROUPINFO(gripper_sim, "GRPS_", 23, SITL, Gripper_Servo), + // @Group: GRPE_ + // @Path: ./SIM_Gripper_EPM.cpp + AP_SUBGROUPINFO(gripper_epm_sim, "GRPE_", 24, SITL, Gripper_EPM), + AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 97c11888d7..60fc7a39b8 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -3,6 +3,10 @@ #include #include +#include "SIM_Sprayer.h" +#include "SIM_Gripper_Servo.h" +#include "SIM_Gripper_EPM.h" + class DataFlash_Class; namespace SITL { @@ -144,7 +148,7 @@ public: AP_Int16 pin_mask; // for GPIO emulation AP_Float speedup; // simulation speedup AP_Int8 odom_enable; // enable visual odomotry data - + // wind control enum WindType { WIND_TYPE_SQRT = 0, @@ -206,6 +210,11 @@ public: // convert a set of roll rates from body frame to earth frame static Vector3f convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro); + + Sprayer sprayer_sim; + + Gripper_Servo gripper_sim; + Gripper_EPM gripper_epm_sim; }; } // namespace SITL diff --git a/libraries/SITL/SITL_Input.h b/libraries/SITL/SITL_Input.h new file mode 100644 index 0000000000..bd563eb845 --- /dev/null +++ b/libraries/SITL/SITL_Input.h @@ -0,0 +1,18 @@ +#pragma once + +#include + +/* + structure passed in giving servo positions as PWM values in + microseconds +*/ +struct sitl_input { + uint16_t servos[16]; + struct { + float speed; // m/s + float direction; // degrees 0..360 + float turbulence; + float dir_z; //degrees -90..90 + } wind; +}; +