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             :   multicopter simulator class
      17             : */
      18             : 
      19             : #include "SIM_Multicopter.h"
      20             : #include <AP_Motors/AP_Motors.h>
      21             : 
      22             : #include <stdio.h>
      23             : 
      24             : using namespace SITL;
      25             : 
      26         909 : MultiCopter::MultiCopter(const char *frame_str) :
      27         909 :     Aircraft(frame_str)
      28             : {
      29         909 :     frame = Frame::create_frame(frame_str);
      30         909 :     if (frame == nullptr) {
      31           0 :         printf("Frame '%s' not found or insufficient memory", frame_str);
      32           0 :         exit(1);
      33             :     }
      34             : 
      35         909 :     frame->init(frame_str);
      36        1818 :     battery.setup(frame->get_model_batt_capacity_ah(),
      37         909 :                   frame->get_model_batt_resistance_ohm(),
      38         909 :                   frame->get_model_batt_max_voltage(),
      39             :                   ambient_outside_temperature_degC());
      40             : 
      41         909 :     mass = frame->get_mass();
      42         909 :     frame_height = 0.1;
      43         909 :     ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
      44         909 :     lock_step_scheduled = true;
      45         909 : }
      46             : 
      47             : // calculate rotational and linear accelerations
      48    81459271 : void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
      49             : {
      50    81459271 :     motor_mask |= ((1U<<frame->num_motors)-1U) << frame->motor_offset;
      51    81459271 :     frame->calculate_forces(*this, input, rot_accel, body_accel, rpm);
      52             : 
      53    81459271 :     add_shove_forces(rot_accel, body_accel);
      54    81459271 :     add_twist_forces(rot_accel);
      55             : 
      56             :     // add forces from slung payload or tether payload
      57    81459271 :     add_external_forces(body_accel);
      58    81459271 : }
      59             : 
      60             : /*
      61             :   update the multicopter simulation by one time step
      62             :  */
      63    81459271 : void MultiCopter::update(const struct sitl_input &input)
      64             : {
      65             :     // refresh mass in case SIM_FRM_ parameters have changed
      66    81459271 :     mass = frame->get_mass();
      67             : 
      68             :     // get wind vector setup
      69    81459271 :     update_wind(input);
      70             : 
      71    81459271 :     Vector3f rot_accel;
      72             : 
      73    81459271 :     calculate_forces(input, rot_accel, accel_body);
      74             :     // simulated clamp holding vehicle down
      75    81459271 :     if (clamp.clamped(*this, input)) {
      76       74045 :         rot_accel.zero();
      77       74045 :         accel_body.zero();
      78             :     }
      79             : 
      80    81459271 :     update_battery();
      81    81459271 :     update_dynamics(rot_accel);
      82    81459271 :     update_external_payload(input);
      83             : 
      84             :     // update lat/lon/altitude
      85    81459271 :     update_position();
      86    81459271 :     time_advance();
      87             : 
      88             :     // update magnetic field
      89    81459271 :     update_mag_field_bf();
      90    81459271 : }
      91             : 
      92    81459271 : void MultiCopter::update_battery() {
      93    81459271 :     if (frame->battery_changed()) {
      94             :         // battery model changed via SIM_FRM_ parameters
      95        1818 :         battery.setup(frame->get_model_batt_capacity_ah(),
      96         909 :                       frame->get_model_batt_resistance_ohm(),
      97         909 :                       frame->get_model_batt_max_voltage(),
      98             :                       ambient_outside_temperature_degC());
      99             :     }
     100    81459271 :     battery.maybe_reset(sitl->batt_voltage, sitl->batt_capacity_ah, sitl->batt_resistance);
     101    81459271 :     battery_voltage = battery.get_voltage();
     102    81459271 :     battery_current = frame->get_current_amp();
     103    81459271 :     battery_temperature_degC = battery.get_temperature_degC();
     104             : 
     105    81459271 :     const uint64_t now_us = AP_HAL::micros64();
     106    81459271 :     battery.consume_energy(battery_current, now_us);
     107    81459271 : }