SITL: added IC engine simulation

libraries/SITL/SIM_ICEngine.cpp

@@ -0,0 +1,115 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  simple internal combustion engine simulator class
+*/
+
+#include "SIM_ICEngine.h"
+#include <stdio.h>
+
+using namespace SITL;
+
+/*
+  update engine state, returning power output from 0 to 1
+ */
+float ICEngine::update(const Aircraft::sitl_input &input)
+{
+    bool have_ignition = ignition_servo>=0;
+    bool have_choke = choke_servo>=0;
+    bool have_starter = starter_servo>=0;
+    float throttle_demand = (input.servos[throttle_servo]-1000) * 0.001f;
+
+    state.ignition = have_ignition?input.servos[ignition_servo]>1700:true;
+    state.choke = have_choke?input.servos[choke_servo]>1700:false;
+    state.starter = have_starter?input.servos[starter_servo]>1700:false;
+
+    uint64_t now = AP_HAL::micros64();
+    float dt = (now - last_update_us) * 1.0e-6f;
+    float max_change = slew_rate * 0.01f * dt;
+    
+    if (!have_starter) {
+        // always on
+        last_output = throttle_demand;
+        return last_output;
+    }
+
+    if (state.value != last_state.value) {
+        printf("choke:%u starter:%u ignition:%u\n",
+               (unsigned)state.choke,
+               (unsigned)state.starter,
+               (unsigned)state.ignition);
+    }
+    
+    if (have_ignition && !state.ignition) {
+        // engine is off
+        if (!state.starter) {
+            goto engine_off;
+        }
+        // give 10% when on starter alone without ignition
+        last_update_us = now;
+        throttle_demand = 0.1;
+        goto output;
+    }
+    if (have_choke && state.choke && now - start_time_us > 1000*1000UL) {
+        // engine is choked, only run for 1s
+        goto engine_off;
+    }
+    if (last_output <= 0 && !state.starter) {
+        // not started
+        goto engine_off;
+    }
+    if (start_time_us == 0 && state.starter) {
+        if (throttle_demand > 0.2) {
+            printf("too much throttle to start: %.2f\n", throttle_demand);
+        } else {
+            // start the motor
+            if (start_time_us == 0) {
+                printf("Engine started\n");
+            }
+            start_time_us = now;
+        }
+    }
+    if (start_time_us != 0 && state.starter) {
+        uint32_t starter_time_us = (now - start_time_us);
+        if (starter_time_us > 3000*1000UL) {
+            printf("Starter overheat\n");            
+        }
+    }
+
+output:
+    if (start_time_us != 0 && throttle_demand < 0.01) {
+        // even idling it gives some thrust
+        throttle_demand = 0.01;
+    }
+
+    last_output = constrain_float(throttle_demand, last_output-max_change, last_output+max_change);
+    last_output = constrain_float(last_output, 0, 1);
+    
+    last_update_us = now;
+    last_state = state;
+    return last_output;
+
+engine_off:
+    if (start_time_us != 0) {
+        printf("Engine stopped\n");
+    }
+    last_update_us = AP_HAL::micros64();
+    start_time_us = 0;
+    last_output = 0;
+    last_state = state;
+    start_time_us = 0;
+    return 0;
+}

libraries/SITL/SIM_ICEngine.h

@@ -0,0 +1,58 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  simple internal combustion motor simulation class
+*/
+
+#pragma once
+
+#include "SIM_Aircraft.h"
+
+namespace SITL {
+
+class ICEngine {
+public:
+    const uint8_t throttle_servo;
+    const int8_t choke_servo;
+    const int8_t ignition_servo;
+    const int8_t starter_servo;
+    const float slew_rate; // percent-per-second
+
+    ICEngine(uint8_t _throttle, int8_t _choke, int8_t _ignition, int8_t _starter, float _slew_rate) :
+        throttle_servo(_throttle),
+        choke_servo(_choke),
+        ignition_servo(_ignition),
+        starter_servo(_starter),
+        slew_rate(_slew_rate)
+    {}
+
+    // update motor state
+    float update(const struct Aircraft::sitl_input &input);
+
+private:
+    float last_output;
+    uint64_t start_time_us;
+    uint64_t last_update_us;
+    union state {
+        struct {
+            bool choke:1;
+            bool ignition:1;
+            bool starter:1;
+        };
+        uint8_t value;
+    } state, last_state;
+};
+}

libraries/SITL/SIM_Plane.cpp

@@ -52,6 +52,9 @@ Plane::Plane(const char *home_str, const char *frame_str) :
     }
 
     ground_behavior = GROUND_BEHAVIOR_FWD_ONLY;
+    if (strstr(frame_str, "-ice")) {
+        ice_engine = true;
+    }
 }
 
 /*
@@ -236,6 +239,10 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel
     
     float thrust     = throttle;
 
+    if (ice_engine) {
+        thrust = icengine.update(input);
+    }
+
     // calculate angle of attack
     angle_of_attack = atan2f(velocity_air_bf.z, velocity_air_bf.x);
     beta = atan2f(velocity_air_bf.y,velocity_air_bf.x);

libraries/SITL/SIM_Plane.h

@@ -20,6 +20,7 @@
 #pragma once
 
 #include "SIM_Aircraft.h"
+#include "SIM_ICEngine.h"
 
 namespace SITL {
 
@@ -95,6 +96,9 @@ protected:
     bool elevons;
     bool vtail;
     bool reverse_elevator_rudder;
+    bool ice_engine;
+
+    ICEngine icengine{2, 4, 5, 6, 100};
 
     float liftCoeff(float alpha) const;
     float dragCoeff(float alpha) const;