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             :   simple quadplane simulator class
      17             : */
      18             : 
      19             : #include "SIM_QuadPlane.h"
      20             : 
      21             : #include <stdio.h>
      22             : 
      23             : using namespace SITL;
      24             : 
      25         201 : QuadPlane::QuadPlane(const char *frame_str) :
      26         201 :     Plane(frame_str)
      27             : {
      28             :     // default to X frame
      29         201 :     const char *frame_type = "x";
      30         201 :     uint8_t motor_offset = 4;
      31             : 
      32         201 :     ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
      33             : 
      34         201 :     if (strstr(frame_str, "-octa-quad-cor")) {
      35           0 :         frame_type = "octa-quad-cor";
      36         201 :     } else if (strstr(frame_str, "-octa-quad-cw-cor")) {
      37           0 :         frame_type = "octa-quad-cw-cor";
      38         201 :     } else if (strstr(frame_str, "-octa-quad")) {
      39           0 :         frame_type = "octa-quad";
      40         201 :     } else if (strstr(frame_str, "-octaquad")) {
      41           0 :         frame_type = "octa-quad";
      42         201 :     } else if (strstr(frame_str, "-octa")) {
      43           0 :         frame_type = "octa";
      44         201 :     } else if (strstr(frame_str, "-hexax")) {
      45           0 :         frame_type = "hexax";
      46         201 :     } else if (strstr(frame_str, "-hexa")) {
      47           0 :         frame_type = "hexa";
      48         201 :     } else if (strstr(frame_str, "-plus")) {
      49           0 :         frame_type = "+";
      50         201 :     } else if (strstr(frame_str, "-y6")) {
      51           0 :         frame_type = "y6";
      52         201 :     } else if (strstr(frame_str, "-tri")) {
      53           3 :         frame_type = "tri";
      54         198 :     } else if (strstr(frame_str, "-tilttrivec")) {
      55           2 :         frame_type = "tilttrivec";
      56             :         // fwd motor gives zero thrust
      57           2 :         thrust_scale = 0;
      58         196 :     } else if (strstr(frame_str, "-tilthvec")) {
      59           3 :         frame_type = "tilthvec";
      60         193 :     } else if (strstr(frame_str, "-tilttri")) {
      61           2 :         frame_type = "tilttri";
      62             :         // fwd motor gives zero thrust
      63           2 :         thrust_scale = 0;
      64         191 :     } else if (strstr(frame_str, "firefly")) {
      65           2 :         frame_type = "firefly";
      66             :         // elevon style surfaces
      67           2 :         elevons = true;
      68             :         // fwd motor gives zero thrust
      69           2 :         thrust_scale = 0;
      70             :         // vtol motors start at 2
      71           2 :         motor_offset = 2;
      72         189 :     } else if (strstr(frame_str, "-tilt")) {
      73           2 :         frame_type = "tilt";
      74             :         // fwd motor gives zero thrust
      75           2 :         thrust_scale = 0;
      76         187 :     } else if (strstr(frame_str, "cl84")) {
      77           2 :         frame_type = "tilttri";
      78             :         // fwd motor gives zero thrust
      79           2 :         thrust_scale = 0;
      80         185 :     } else if (strstr(frame_str, "-copter_tailsitter")) {
      81           5 :         frame_type = "+";
      82           5 :         copter_tailsitter = true;
      83           5 :         ground_behavior = GROUND_BEHAVIOR_TAILSITTER;
      84           5 :         thrust_scale *= 1.5;
      85             :     }
      86         201 :     frame = Frame::create_frame(frame_type);
      87         201 :     if (frame == nullptr) {
      88           0 :         printf("Failed to find frame '%s' or insufficient memory\n", frame_type);
      89           0 :         exit(1);
      90             :     }
      91             : 
      92         201 :     if (strstr(frame_str, "cl84")) {
      93             :         // setup retract servos at front
      94           2 :         frame->motors[0].servo_type = Motor::SERVO_RETRACT;
      95           2 :         frame->motors[0].servo_rate = 7*60.0/90; // 7 seconds to change
      96           2 :         frame->motors[1].servo_type = Motor::SERVO_RETRACT;
      97           2 :         frame->motors[1].servo_rate = 7*60.0/90; // 7 seconds to change
      98             :     }
      99             :     
     100             :     // leave first 4 servos free for plane
     101         201 :     frame->motor_offset = motor_offset;
     102             : 
     103             :     // increase mass for plane components
     104         201 :     frame->set_mass_scale(1.5);
     105             : 
     106         201 :     frame->init(frame_str);
     107         402 :     battery.setup(frame->get_model_batt_capacity_ah(),
     108         201 :                   frame->get_model_batt_resistance_ohm(),
     109         201 :                   frame->get_model_batt_max_voltage(),
     110             :                   ambient_outside_temperature_degC());
     111             : 
     112         201 :     mass = frame->get_mass();
     113             : 
     114         201 :     lock_step_scheduled = true;
     115         201 : }
     116             : 
     117             : /*
     118             :   update the quadplane simulation by one time step
     119             :  */
     120    26485960 : void QuadPlane::update(const struct sitl_input &input)
     121             : {
     122             :     // refresh mass in case SIM_FRM_ parameters have changed
     123    26485960 :     mass = frame->get_mass();
     124             : 
     125             :     // get wind vector setup
     126    26485960 :     update_wind(input);
     127             : 
     128             :     // first plane forces
     129    26485960 :     Vector3f rot_accel;
     130    26485960 :     calculate_forces(input, rot_accel);
     131             : 
     132             :     // now quad forces
     133    26485960 :     Vector3f quad_rot_accel;
     134    26485960 :     Vector3f quad_accel_body;
     135             : 
     136    26485960 :     motor_mask |= ((1U<<frame->num_motors)-1U) << frame->motor_offset;
     137    26485960 :     frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body, rpm);
     138             : 
     139             :     // rotate frames for copter tailsitters
     140    26485960 :     if (copter_tailsitter) {
     141      541998 :         quad_rot_accel.rotate(ROTATION_PITCH_270);
     142      541998 :         quad_accel_body.rotate(ROTATION_PITCH_270);
     143             :     }
     144             : 
     145    26485960 :     if (frame->battery_changed()) {
     146             :         // battery model changed via SIM_FRM_ parameters
     147         402 :         battery.setup(frame->get_model_batt_capacity_ah(),
     148         201 :                       frame->get_model_batt_resistance_ohm(),
     149         201 :                       frame->get_model_batt_max_voltage(),
     150             :                       ambient_outside_temperature_degC());
     151             :     }
     152    26485960 :     battery.maybe_reset(sitl->batt_voltage, sitl->batt_capacity_ah, sitl->batt_resistance);
     153    26485960 :     battery_voltage = battery.get_voltage();
     154    26485960 :     battery_current = frame->get_current_amp();
     155    26485960 :     battery_temperature_degC = battery.get_temperature_degC();
     156             : 
     157    26485960 :     const uint64_t now_us = AP_HAL::micros64();
     158    26485960 :     battery.consume_energy(battery_current, now_us);
     159             : 
     160             :     float throttle;
     161    26485960 :     if (reverse_thrust) {
     162           0 :         throttle = filtered_servo_angle(input, 2);
     163             :     } else {
     164    26485960 :         throttle = filtered_servo_range(input, 2);
     165             :     }
     166             :     // assume 20A at full fwd throttle
     167    26485960 :     throttle = fabsf(throttle);
     168    26485960 :     battery_current += 20 * throttle;
     169             :     
     170    26485960 :     rot_accel += quad_rot_accel;
     171    26485960 :     accel_body += quad_accel_body;
     172             : 
     173    26485960 :     update_dynamics(rot_accel);
     174    26485960 :     update_external_payload(input);
     175             : 
     176             :     // update lat/lon/altitude
     177    26485960 :     update_position();
     178    26485960 :     time_advance();
     179             : 
     180             :     // update magnetic field
     181    26485960 :     update_mag_field_bf();
     182    26485960 : }