From 995fb7cd86dee09e498bdbad2f96326db3f7e114 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Jul 2016 18:29:12 +1000 Subject: [PATCH] SITL: added IC engine simulation --- libraries/SITL/SIM_ICEngine.cpp | 115 ++++++++++++++++++++++++++++++++ libraries/SITL/SIM_ICEngine.h | 58 ++++++++++++++++ libraries/SITL/SIM_Plane.cpp | 7 ++ libraries/SITL/SIM_Plane.h | 4 ++ 4 files changed, 184 insertions(+) create mode 100644 libraries/SITL/SIM_ICEngine.cpp create mode 100644 libraries/SITL/SIM_ICEngine.h diff --git a/libraries/SITL/SIM_ICEngine.cpp b/libraries/SITL/SIM_ICEngine.cpp new file mode 100644 index 0000000000..4f5d421d92 --- /dev/null +++ b/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 . + */ +/* + simple internal combustion engine simulator class +*/ + +#include "SIM_ICEngine.h" +#include + +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; +} diff --git a/libraries/SITL/SIM_ICEngine.h b/libraries/SITL/SIM_ICEngine.h new file mode 100644 index 0000000000..83807fc28b --- /dev/null +++ b/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 . + */ +/* + 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; +}; +} diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index f55bd0f087..0aeb8de721 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/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); diff --git a/libraries/SITL/SIM_Plane.h b/libraries/SITL/SIM_Plane.h index 2304c308b0..3ae78e1fdd 100644 --- a/libraries/SITL/SIM_Plane.h +++ b/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;