Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include <AP_Scripting/AP_Scripting_config.h>
      17             : 
      18             : #if AP_SCRIPTING_ENABLED
      19             : 
      20             : // This allows motor roll, pitch, yaw and throttle factors to be changed in flight, allowing vehicle geometry to be changed
      21             : 
      22             : #include "AP_MotorsMatrix_Scripting_Dynamic.h"
      23             : #include <GCS_MAVLink/GCS.h>
      24             : #include <AP_HAL/AP_HAL.h>
      25             : 
      26             : extern const AP_HAL::HAL& hal;
      27             : 
      28             : #define debug_print 0
      29             : 
      30             : // add a motor and give its testing order
      31           0 : bool AP_MotorsMatrix_Scripting_Dynamic::add_motor(uint8_t motor_num, uint8_t testing_order)
      32             : {
      33           0 :     if (initialised_ok()) {
      34             :         // no adding motors after init
      35           0 :         return false;
      36             :     }
      37           0 :     if (motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
      38           0 :         _test_order[motor_num] = testing_order;
      39           0 :         motor_enabled[motor_num] = true;
      40           0 :         return true;
      41             :     }
      42           0 :     return false;
      43             : }
      44             : 
      45           0 : void AP_MotorsMatrix_Scripting_Dynamic::load_factors(const factor_table &new_table)
      46             : {
      47           0 :     WITH_SEMAPHORE(_sem);
      48           0 :     had_table = true;
      49             : 
      50           0 :     memcpy(_roll_factor,new_table.roll,sizeof(_roll_factor));
      51           0 :     memcpy(_pitch_factor,new_table.pitch,sizeof(_pitch_factor));
      52           0 :     memcpy(_yaw_factor,new_table.yaw,sizeof(_yaw_factor));
      53           0 :     memcpy(_throttle_factor,new_table.throttle,sizeof(_throttle_factor));
      54             : 
      55             : #if debug_print
      56             :     hal.console->printf("Got new factors:\n");
      57             :     for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
      58             :         if (motor_enabled[i]) {
      59             :             hal.console->printf("%i - Roll: %0.2f, Pitch %0.2f, Yaw: %0.2f, throttle %0.2f\n",i,_roll_factor[i],_pitch_factor[i],_yaw_factor[i],_throttle_factor[i]);
      60             :         }
      61             :     }
      62             : #endif
      63             : 
      64           0 : }
      65             : 
      66           0 : bool AP_MotorsMatrix_Scripting_Dynamic::init(uint8_t expected_num_motors)
      67             : {
      68           0 :     WITH_SEMAPHORE(_sem);
      69             : 
      70             :     // Check factors have been set
      71           0 :     if (!had_table) {
      72           0 :         return false;
      73             :     }
      74             : 
      75             :     // Make sure the correct number of motors have been added
      76           0 :     uint8_t num_motors = 0;
      77           0 :     for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
      78           0 :         if (motor_enabled[i]) {
      79           0 :             num_motors++;
      80             :         }
      81             :     }
      82             : 
      83           0 :     set_initialised_ok(expected_num_motors == num_motors);
      84             : 
      85           0 :     if (!initialised_ok()) {
      86           0 :         _mav_type = MAV_TYPE_GENERIC;
      87           0 :         return false;
      88             :     }
      89             : 
      90           0 :     switch (num_motors) {
      91           0 :         case 3:
      92           0 :             _mav_type = MAV_TYPE_TRICOPTER;
      93           0 :             break;
      94           0 :         case 4:
      95           0 :             _mav_type = MAV_TYPE_QUADROTOR;
      96           0 :             break;
      97           0 :         case 6:
      98           0 :             _mav_type = MAV_TYPE_HEXAROTOR;
      99           0 :             break;
     100           0 :         case 8:
     101           0 :             _mav_type = MAV_TYPE_OCTOROTOR;
     102           0 :             break;
     103           0 :         case 10:
     104           0 :             _mav_type = MAV_TYPE_DECAROTOR;
     105           0 :             break;
     106           0 :         case 12:
     107           0 :             _mav_type = MAV_TYPE_DODECAROTOR;
     108           0 :             break;
     109           0 :         default:
     110           0 :             _mav_type = MAV_TYPE_GENERIC;
     111             :     }
     112             : 
     113           0 :     set_update_rate(_speed_hz);
     114             : 
     115           0 :     return true;
     116           0 : }
     117             : 
     118             : // output - sends commands to the motors, 
     119             : // Need to take the semaphore to enasure the motor factors are not changed during the mixer calculation
     120           0 : void AP_MotorsMatrix_Scripting_Dynamic::output_to_motors()
     121             : {
     122           0 :     WITH_SEMAPHORE(_sem);
     123             : 
     124             :     // call the base class ouput function
     125           0 :     AP_MotorsMatrix::output_to_motors();
     126           0 : }
     127             : 
     128             : // singleton instance
     129             : AP_MotorsMatrix_Scripting_Dynamic *AP_MotorsMatrix_Scripting_Dynamic::_singleton;
     130             : 
     131             : #endif // AP_SCRIPTING_ENABLED