diff --git a/libraries/AP_Motors/AP_Motors6DOF.h b/libraries/AP_Motors/AP_Motors6DOF.h index fc6417db2c..14ed738fa9 100644 --- a/libraries/AP_Motors/AP_Motors6DOF.h +++ b/libraries/AP_Motors/AP_Motors6DOF.h @@ -29,8 +29,6 @@ public: SUB_FRAME_CUSTOM } sub_frame_t; - const char* get_frame_string() const override { return _frame_class_string; }; - // Override parent void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override; diff --git a/libraries/AP_Motors/AP_MotorsCoax.h b/libraries/AP_Motors/AP_MotorsCoax.h index d02e3bcd98..e3b240b438 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.h +++ b/libraries/AP_Motors/AP_MotorsCoax.h @@ -49,8 +49,6 @@ public: // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict uint16_t get_motor_mask() override; - const char* get_frame_string() const override { return "COAX"; } - protected: // output - sends commands to the motors void output_armed_stabilizing() override; @@ -58,4 +56,6 @@ protected: float _actuator_out[NUM_ACTUATORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range float _thrust_yt_ccw; float _thrust_yt_cw; + + const char* _get_frame_string() const override { return "COAX"; } }; diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index b1492db9dd..4467229977 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -159,8 +159,6 @@ public: // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; - const char* get_frame_string() const override { return "HELI"; } - protected: // manual servo modes (used for setup) @@ -227,6 +225,8 @@ protected: // updates the takeoff collective flag indicating that current collective is greater than collective required to indicate takeoff. void update_takeoff_collective_flag(float coll_out); + const char* _get_frame_string() const override { return "HELI"; } + // enum values for HOVER_LEARN parameter enum HoverLearn { HOVER_LEARN_DISABLED = 0, diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.h b/libraries/AP_Motors/AP_MotorsHeli_Dual.h index e8274d71a5..641e28a76d 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.h @@ -95,8 +95,6 @@ public: // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; - const char* get_frame_string() const override { return "HELI_DUAL"; } - protected: // init_outputs @@ -111,6 +109,8 @@ protected: // move_actuators - moves swash plate to attitude of parameters passed in void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) override; + const char* _get_frame_string() const override { return "HELI_DUAL"; } + // objects we depend upon AP_MotorsHeli_Swash _swashplate1; // swashplate1 AP_MotorsHeli_Swash _swashplate2; // swashplate2 diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.h b/libraries/AP_Motors/AP_MotorsHeli_Quad.h index e84ceccd08..89c8d24936 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.h @@ -74,8 +74,6 @@ public: // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; - const char* get_frame_string() const override { return "HELI_QUAD"; } - protected: // init_outputs @@ -90,6 +88,8 @@ protected: // move_actuators - moves swash plate to attitude of parameters passed in void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) override; + const char* _get_frame_string() const override { return "HELI_QUAD"; } + // rate factors float _rollFactor[AP_MOTORS_HELI_QUAD_NUM_MOTORS]; float _pitchFactor[AP_MOTORS_HELI_QUAD_NUM_MOTORS]; diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index 26db4bc779..4a355d0530 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -76,9 +76,6 @@ public: // return the pitch factor of any motor float get_pitch_factor(uint8_t i) override { return _pitch_factor[i]; } - const char* get_frame_string() const override { return _frame_class_string; } - const char* get_type_string() const override { return _frame_type_string; } - // disable the use of motor torque to control yaw. Used when an external mechanism such // as vectoring is used for yaw control void disable_yaw_torque(void) override; @@ -134,6 +131,9 @@ protected: // call vehicle supplied thrust compensation if set void thrust_compensation(void) override; + const char* _get_frame_string() const override { return _frame_class_string; } + const char* get_type_string() const override { return _frame_type_string; } + float _roll_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to roll float _pitch_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to pitch float _yaw_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to yaw (normally 1 or -1) diff --git a/libraries/AP_Motors/AP_MotorsMatrix_6DoF_Scripting.h b/libraries/AP_Motors/AP_MotorsMatrix_6DoF_Scripting.h index 1a3cc9f307..86b973b093 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix_6DoF_Scripting.h +++ b/libraries/AP_Motors/AP_MotorsMatrix_6DoF_Scripting.h @@ -37,8 +37,6 @@ public: // if the expected number of motors have been setup then set as initalized bool init(uint8_t expected_num_motors) override; - const char* get_frame_string() const override { return "6DoF scripting"; } - protected: // output - sends commands to the motors void output_armed_stabilizing() override; @@ -46,6 +44,8 @@ protected: // nothing to do for setup, scripting will mark as initalized when done void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override {}; + const char* _get_frame_string() const override { return "6DoF scripting"; } + float _forward_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to forward thrust float _right_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to right thrust diff --git a/libraries/AP_Motors/AP_MotorsMatrix_Scripting_Dynamic.h b/libraries/AP_Motors/AP_MotorsMatrix_Scripting_Dynamic.h index c6373a64fc..70879d0040 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix_Scripting_Dynamic.h +++ b/libraries/AP_Motors/AP_MotorsMatrix_Scripting_Dynamic.h @@ -43,14 +43,14 @@ public: // output - sends commands to the motors void output_to_motors() override; - const char* get_frame_string() const override { return "Dynamic Matrix"; } - protected: // Do not apply thrust compensation, this is used by Quadplane tiltrotors // assume the compensation is done in the mixer and should not be done by quadplane void thrust_compensation(void) override {}; + const char* _get_frame_string() const override { return "Dynamic Matrix"; } + private: // True when received a factors table, will only init having received a table diff --git a/libraries/AP_Motors/AP_MotorsSingle.h b/libraries/AP_Motors/AP_MotorsSingle.h index 32317e9a10..65b5389ea7 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.h +++ b/libraries/AP_Motors/AP_MotorsSingle.h @@ -49,12 +49,12 @@ public: // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict uint16_t get_motor_mask() override; - const char* get_frame_string() const override { return "SINGLE"; } - protected: // output - sends commands to the motors void output_armed_stabilizing() override; + const char* _get_frame_string() const override { return "SINGLE"; } + int16_t _throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900 float _actuator_out[NUM_ACTUATORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range float _thrust_out; diff --git a/libraries/AP_Motors/AP_MotorsTailsitter.h b/libraries/AP_Motors/AP_MotorsTailsitter.h index bf9f8a322d..d00b7815f8 100644 --- a/libraries/AP_Motors/AP_MotorsTailsitter.h +++ b/libraries/AP_Motors/AP_MotorsTailsitter.h @@ -33,12 +33,12 @@ public: // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict uint16_t get_motor_mask() override; - const char* get_frame_string() const override { return "TAILSITTER"; } - protected: // calculate motor outputs void output_armed_stabilizing() override; + const char* _get_frame_string() const override { return "TAILSITTER"; } + // calculated outputs float _throttle; // 0..1 float _tilt_left; // -1..1 diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index df3a0b895e..880377268b 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -54,15 +54,15 @@ public: // using copter motors for forward flight float get_roll_factor(uint8_t i) override; - const char* get_frame_string() const override { return "TRI"; } - protected: // output - sends commands to the motors void output_armed_stabilizing() override; // call vehicle supplied thrust compensation if set void thrust_compensation(void) override; - + + const char* _get_frame_string() const override { return "TRI"; } + // parameters float _pivot_angle; // Angle of yaw pivot diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index fb358031be..87ecf3b641 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -52,7 +52,11 @@ void AP_Motors::get_frame_and_type_string(char *buffer, uint8_t buflen) const { const char *frame_str = get_frame_string(); const char *type_str = get_type_string(); - if (type_str != nullptr && strlen(type_str)) { + if (type_str != nullptr && strlen(type_str) +#if AP_SCRIPTING_ENABLED + && custom_frame_string == nullptr +#endif + ) { hal.util->snprintf(buffer, buflen, "Frame: %s/%s", frame_str, type_str); } else { hal.util->snprintf(buffer, buflen, "Frame: %s", frame_str); @@ -227,6 +231,31 @@ bool AP_Motors::is_digital_pwm_type() const return false; } +// return string corresponding to frame_class +const char* AP_Motors::get_frame_string() const +{ +#if AP_SCRIPTING_ENABLED + if (custom_frame_string != nullptr) { + return custom_frame_string; + } +#endif + return _get_frame_string(); +} + +#if AP_SCRIPTING_ENABLED +// set custom frame string +void AP_Motors::set_frame_string(const char * str) { + if (custom_frame_string != nullptr) { + return; + } + const size_t len = strlen(str)+1; + custom_frame_string = new char[len]; + if (custom_frame_string != nullptr) { + strncpy(custom_frame_string, str, len); + } +} +#endif + namespace AP { AP_Motors *motors() { diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 37f8d2afaa..524f9ff81b 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -73,7 +73,7 @@ public: }; // return string corresponding to frame_class - virtual const char* get_frame_string() const = 0; + const char* get_frame_string() const; enum motor_frame_type { MOTOR_FRAME_TYPE_PLUS = 0, @@ -95,8 +95,6 @@ public: MOTOR_FRAME_TYPE_Y4 = 19, //Y4 Quadrotor frame }; - // return string corresponding to frame_type - virtual const char* get_type_string() const { return ""; } // returns a formatted string into buffer, e.g. "QUAD/X" void get_frame_and_type_string(char *buffer, uint8_t buflen) const; @@ -256,6 +254,10 @@ public: // direct motor write virtual void rc_write(uint8_t chan, uint16_t pwm); +#if AP_SCRIPTING_ENABLED + void set_frame_string(const char * str); +#endif + protected: // output functions that should be overloaded by child classes virtual void output_armed_stabilizing() = 0; @@ -331,6 +333,17 @@ protected: PWM_TYPE_DSHOT1200 = 7, PWM_TYPE_PWM_RANGE = 8 }; + // return string corresponding to frame_class + virtual const char* _get_frame_string() const = 0; + + // return string corresponding to frame_type + virtual const char* get_type_string() const { return ""; } + +#if AP_SCRIPTING_ENABLED + // Custom frame string set from scripting + char* custom_frame_string; +#endif + private: bool _armed; // 0 if disarmed, 1 if armed diff --git a/libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp b/libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp index 00da1a15f6..f4aa6bd569 100644 --- a/libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp +++ b/libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp @@ -39,7 +39,7 @@ public: void init(motor_frame_class frame_class, motor_frame_type frame_type) override {}; void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type) override {}; void output_test_seq(uint8_t motor_seq, int16_t pwm) override {}; - const char* get_frame_string() const override { return "TEST"; } + const char* _get_frame_string() const override { return "TEST"; } void output_armed_stabilizing() override {}; void output_to_motors() override {};