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 internal combustion engine simulator class
      17             : */
      18             : 
      19             : #include "SIM_ICEngine.h"
      20             : #include <stdio.h>
      21             : 
      22             : using namespace SITL;
      23             : 
      24             : #include <GCS_MAVLink/GCS.h>
      25             : 
      26             : /*
      27             :   update engine state, returning power output from 0 to 1
      28             :  */
      29     1398922 : float ICEngine::update(const struct sitl_input &input)
      30             : {
      31     1398922 :     bool have_ignition = ignition_servo>=0;
      32     1398922 :     bool have_choke = choke_servo>=0;
      33     1398922 :     bool have_starter = starter_servo>=0;
      34     1398922 :     float throttle_demand = (input.servos[throttle_servo]-1000) * 0.001f;
      35     1398922 :     if (throttle_reversed) {
      36     1398922 :         throttle_demand = 1 - throttle_demand;
      37             :     }
      38             : 
      39     1398922 :     state.ignition = have_ignition?input.servos[ignition_servo]>1700:true;
      40     1398922 :     state.choke = have_choke?input.servos[choke_servo]>1700:false;
      41     1398922 :     state.starter = have_starter?input.servos[starter_servo]>1700:false;
      42             : 
      43     1398922 :     uint64_t now = AP_HAL::micros64();
      44     1398922 :     float dt = (now - last_update_us) * 1.0e-6f;
      45     1398922 :     float max_change = slew_rate * 0.01f * dt;
      46             :     
      47     1398922 :     if (!have_starter) {
      48             :         // always on
      49           0 :         last_output = throttle_demand;
      50           0 :         return last_output;
      51             :     }
      52             : 
      53     1398922 :     if (state.value != last_state.value) {
      54          62 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SIM_ICEngine: choke:%u starter:%u ignition:%u",
      55             :                       (unsigned)state.choke,
      56             :                       (unsigned)state.starter,
      57             :                       (unsigned)state.ignition);
      58             :     }
      59             : 
      60     1398922 :     if (have_ignition && !state.ignition) {
      61             :         // engine is off
      62      286167 :         if (!state.starter) {
      63      286167 :             goto engine_off;
      64             :         }
      65             :         // give 10% when on starter alone without ignition
      66           0 :         last_update_us = now;
      67           0 :         throttle_demand = 0.1;
      68           0 :         goto output;
      69             :     }
      70     1112755 :     if (have_choke && state.choke && now - start_time_us > 1000*1000UL) {
      71             :         // engine is choked, only run for 1s
      72           0 :         goto engine_off;
      73             :     }
      74     1112755 :     if (last_output <= 0 && !state.starter) {
      75             :         // not started
      76       31809 :         goto engine_off;
      77             :     }
      78     1080946 :     if (start_time_us == 0 && state.starter) {
      79          11 :         if (throttle_demand > 0.2) {
      80           0 :             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SIM_ICEngine: too much throttle to start: %.2f", throttle_demand);
      81             :         } else {
      82             :             // start the motor
      83          11 :             if (start_time_us == 0) {
      84          11 :                 GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SIM_ICEngine: Engine started");
      85             :             }
      86          11 :             start_time_us = now;
      87             :         }
      88             :     }
      89     1080946 :     if (start_time_us != 0 && state.starter) {
      90       72981 :         uint32_t starter_time_us = (now - start_time_us);
      91       72981 :         if (starter_time_us > 3000*1000UL && !overheat) {
      92          14 :             overheat = true;
      93          14 :             GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SIM_ICEngine: Starter overheat");
      94             :         }
      95       72981 :     } else {
      96     1007965 :         overheat = false;
      97             :     }
      98             : 
      99     1080946 : output:
     100     1080946 :     if (start_time_us != 0 && throttle_demand < 0.01) {
     101             :         // even idling it gives some thrust
     102      379511 :         throttle_demand = 0.01;
     103             :     }
     104             : 
     105     1080946 :     last_output = constrain_float(throttle_demand, last_output-max_change, last_output+max_change);
     106     1080946 :     last_output = constrain_float(last_output, 0, 1);
     107             :     
     108     1080946 :     last_update_us = now;
     109     1080946 :     last_state = state;
     110     1080946 :     return last_output;
     111             : 
     112      317976 : engine_off:
     113      317976 :     if (start_time_us != 0) {
     114          11 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SIM_ICEngine: Engine stopped");
     115             :     }
     116      317976 :     last_update_us = AP_HAL::micros64();
     117      317976 :     start_time_us = 0;
     118      317976 :     last_output = 0;
     119      317976 :     last_state = state;
     120      317976 :     start_time_us = 0;
     121      317976 :     return 0;
     122             : }