ardupilot/libraries/SITL/SIM_ICEngine.cpp

121 lines
3.6 KiB
C++

/*
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 struct 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;
if (throttle_reversed) {
throttle_demand = 1 - throttle_demand;
}
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 && !overheat) {
overheat = true;
printf("Starter overheat\n");
}
} else {
overheat = false;
}
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;
}