diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index d28cc47d1f..e11f5df320 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -26,6 +26,11 @@ void AP_MotorsMatrix::init(motor_frame_class frame_class, motor_frame_type frame _last_frame_class = frame_class; _last_frame_type = frame_type; + if (frame_class == MOTOR_FRAME_SCRIPTING_MATRIX) { + // if Scripting frame class, do nothing scripting must call its own dedicated init function + return; + } + // setup the motors setup_motors(frame_class, frame_type); @@ -33,6 +38,63 @@ void AP_MotorsMatrix::init(motor_frame_class frame_class, motor_frame_type frame set_update_rate(_speed_hz); } +#ifdef ENABLE_SCRIPTING +// dedicated init for lua scripting +bool AP_MotorsMatrix::init(uint8_t expected_num_motors) +{ + if (_last_frame_class != MOTOR_FRAME_SCRIPTING_MATRIX) { + // not the correct class + return false; + } + + // Make sure the correct number of motors have been added + uint8_t num_motors = 0; + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { + if (motor_enabled[i]) { + num_motors++; + } + } + + set_initialised_ok(expected_num_motors == num_motors); + + if (!initialised_ok()) { + _mav_type = MAV_TYPE_GENERIC; + return false; + } + + switch (num_motors) { + case 3: + _mav_type = MAV_TYPE_TRICOPTER; + break; + case 4: + _mav_type = MAV_TYPE_QUADROTOR; + break; + case 6: + _mav_type = MAV_TYPE_HEXAROTOR; + break; + case 8: + _mav_type = MAV_TYPE_OCTOROTOR; + break; + case 10: + _mav_type = MAV_TYPE_DECAROTOR; + break; + case 12: + _mav_type = MAV_TYPE_DODECAROTOR; + break; + default: + _mav_type = MAV_TYPE_GENERIC; + } + + normalise_rpy_factors(); + + set_update_rate(_speed_hz); + + set_initialised_ok(true); + + return true; +} +#endif // ENABLE_SCRIPTING + // set update rate to motors - a value in hertz void AP_MotorsMatrix::set_update_rate(uint16_t speed_hz) { @@ -55,14 +117,9 @@ void AP_MotorsMatrix::set_frame_class_and_type(motor_frame_class frame_class, mo if (armed() || (frame_class == _last_frame_class && _last_frame_type == frame_type)) { return; } - _last_frame_class = frame_class; - _last_frame_type = frame_type; - // setup the motors - setup_motors(frame_class, frame_type); + init(frame_class, frame_type); - // enable fast channels or instant pwm - set_update_rate(_speed_hz); } void AP_MotorsMatrix::output_to_motors() @@ -429,6 +486,11 @@ bool AP_MotorsMatrix::output_test_num(uint8_t output_channel, int16_t pwm) // add_motor void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order) { + if (initialised_ok()) { + // do not allow motors to be set if the current frame type has init correctly + return; + } + // ensure valid motor number is provided if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) { @@ -486,7 +548,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty for (int8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { remove_motor(i); } - + set_initialised_ok(false); bool success = true; switch (frame_class) { @@ -988,3 +1050,6 @@ void AP_MotorsMatrix::disable_yaw_torque(void) _yaw_factor[i] = 0; } } + +// singleton instance +AP_MotorsMatrix *AP_MotorsMatrix::_singleton; diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index 127483e47e..e200c3a466 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -17,11 +17,26 @@ public: /// Constructor AP_MotorsMatrix(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMulticopter(loop_rate, speed_hz) - {}; + { + if (_singleton != nullptr) { + AP_HAL::panic("AP_MotorsMatrix must be singleton"); + } + _singleton = this; + }; + + // get singleton instance + static AP_MotorsMatrix *get_singleton() { + return _singleton; + } // init void init(motor_frame_class frame_class, motor_frame_type frame_type) override; +#ifdef ENABLE_SCRIPTING + // Init to be called from scripting + bool init(uint8_t expected_num_motors); +#endif // ENABLE_SCRIPTING + // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus) void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type) override; @@ -59,6 +74,9 @@ public: // as vectoring is used for yaw control void disable_yaw_torque(void) override; + // add_motor using raw roll, pitch, throttle and yaw factors + void add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order); + protected: // output - sends commands to the motors void output_armed_stabilizing() override; @@ -66,9 +84,6 @@ protected: // check for failed motor void check_for_failed_motor(float throttle_thrust_best); - // add_motor using raw roll, pitch, throttle and yaw factors - void add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order); - // add_motor using just position and yaw_factor (or prop direction) void add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order); @@ -98,4 +113,7 @@ protected: // motor failure handling float _thrust_rpyt_out_filt[AP_MOTORS_MAX_NUM_MOTORS]; // filtered thrust outputs with 1 second time constant uint8_t _motor_lost_index; // index number of the lost motor + +private: + static AP_MotorsMatrix *_singleton; }; diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 85fe94f6cd..4ba6442d6c 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -45,6 +45,7 @@ public: MOTOR_FRAME_DODECAHEXA = 12, MOTOR_FRAME_HELI_QUAD = 13, MOTOR_FRAME_DECA = 14, + MOTOR_FRAME_SCRIPTING_MATRIX = 15, }; enum motor_frame_type { MOTOR_FRAME_TYPE_PLUS = 0,