mirror of https://github.com/ArduPilot/ardupilot
121 lines
3.6 KiB
C++
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;
|
|
}
|