mirror of https://github.com/ArduPilot/ardupilot
SITL: new Loweheiser generator/efi sim
Co-authored-by: Joshua Henderson <hendjoshsr71@gmail.com> SITL: break out common GeneratorEngine code
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
940b6fd1fb
commit
e8708227a1
|
|
@ -1014,6 +1014,13 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
|
|||
richenpower->update(input);
|
||||
}
|
||||
|
||||
#if AP_SIM_LOWEHEISER_ENABLED
|
||||
// update Loweheiser generator
|
||||
if (loweheiser) {
|
||||
loweheiser->update();
|
||||
}
|
||||
#endif
|
||||
|
||||
if (fetteconewireesc) {
|
||||
fetteconewireesc->update(*this);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@
|
|||
#include "SIM_Parachute.h"
|
||||
#include "SIM_Precland.h"
|
||||
#include "SIM_RichenPower.h"
|
||||
#include "SIM_Loweheiser.h"
|
||||
#include "SIM_FETtecOneWireESC.h"
|
||||
#include "SIM_I2C.h"
|
||||
#include "SIM_Buzzer.h"
|
||||
|
|
@ -146,6 +147,9 @@ public:
|
|||
void set_parachute(Parachute *_parachute) { parachute = _parachute; }
|
||||
void set_richenpower(RichenPower *_richenpower) { richenpower = _richenpower; }
|
||||
void set_adsb(class ADSB *_adsb) { adsb = _adsb; }
|
||||
#if AP_SIM_LOWEHEISER_ENABLED
|
||||
void set_loweheiser(Loweheiser *_loweheiser) { loweheiser = _loweheiser; }
|
||||
#endif
|
||||
void set_fetteconewireesc(FETtecOneWireESC *_fetteconewireesc) { fetteconewireesc = _fetteconewireesc; }
|
||||
void set_ie24(IntelligentEnergy24 *_ie24) { ie24 = _ie24; }
|
||||
void set_gripper_servo(Gripper_Servo *_gripper) { gripper = _gripper; }
|
||||
|
|
@ -341,6 +345,9 @@ private:
|
|||
Gripper_EPM *gripper_epm;
|
||||
Parachute *parachute;
|
||||
RichenPower *richenpower;
|
||||
#if AP_SIM_LOWEHEISER_ENABLED
|
||||
Loweheiser *loweheiser;
|
||||
#endif
|
||||
FETtecOneWireESC *fetteconewireesc;
|
||||
|
||||
IntelligentEnergy24 *ie24;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,48 @@
|
|||
#include "SIM_GeneratorEngine.h"
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
using namespace SITL;
|
||||
|
||||
void SIM_GeneratorEngine::update()
|
||||
{
|
||||
if (current_current > 1 && is_zero(desired_rpm)) {
|
||||
AP_HAL::panic("Generator stalled due to high current demand (%u > 1)", (unsigned)current_current);
|
||||
} else if (current_current > max_current) {
|
||||
AP_HAL::panic("Generator stalled due to high current demand (run)");
|
||||
}
|
||||
|
||||
// linear degradation in RPM up to maximum load
|
||||
if (!is_zero(desired_rpm)) {
|
||||
desired_rpm -= 1500 * (current_current/max_current);
|
||||
}
|
||||
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
|
||||
const float max_slew_rpm = max_slew_rpm_per_second * ((now - last_rpm_update_ms) / 1000.0f);
|
||||
last_rpm_update_ms = now;
|
||||
const float rpm_delta = current_rpm - desired_rpm;
|
||||
if (rpm_delta > 0) {
|
||||
current_rpm -= MIN(max_slew_rpm, rpm_delta);
|
||||
} else {
|
||||
current_rpm += MIN(max_slew_rpm, abs(rpm_delta));
|
||||
}
|
||||
|
||||
// update the temperature
|
||||
const uint32_t time_delta_ms = now - last_heat_update_ms;
|
||||
last_heat_update_ms = now;
|
||||
|
||||
constexpr float heat_environment_loss_factor = 0.15f;
|
||||
|
||||
const float factor = 0.0035;
|
||||
temperature += (current_rpm * time_delta_ms * (1/1000.0f) * factor);
|
||||
// cap the heat of the motor:
|
||||
temperature = MIN(temperature, 150);
|
||||
// now lose some heat to the environment
|
||||
const float heat_loss = ((temperature * heat_environment_loss_factor * (time_delta_ms * (1/1000.0f)))); // lose some % of heat per second
|
||||
// gcs().send_text(MAV_SEVERITY_INFO, "heat=%f loss=%f", temperature, heat_loss);
|
||||
temperature -= heat_loss;
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
namespace SITL {
|
||||
|
||||
class SIM_GeneratorEngine
|
||||
{
|
||||
public:
|
||||
void update();
|
||||
|
||||
// input variables:
|
||||
float desired_rpm;
|
||||
float current_current;
|
||||
float max_current;
|
||||
float max_slew_rpm_per_second;
|
||||
float max_rpm = 8000;
|
||||
|
||||
// output variables:
|
||||
float current_rpm;
|
||||
float temperature = 20;
|
||||
|
||||
private:
|
||||
uint32_t last_rpm_update_ms;
|
||||
uint32_t last_heat_update_ms;
|
||||
};
|
||||
|
||||
}; // namespace SITL
|
||||
|
|
@ -0,0 +1,344 @@
|
|||
/*
|
||||
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/>.
|
||||
*/
|
||||
/*
|
||||
Simulator for the Loweheiser generators
|
||||
*/
|
||||
|
||||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_LOWEHEISER_ENABLED
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#include "SIM_Loweheiser.h"
|
||||
#include "SITL.h"
|
||||
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <errno.h>
|
||||
|
||||
using namespace SITL;
|
||||
|
||||
Loweheiser::Loweheiser() : SerialDevice::SerialDevice()
|
||||
{
|
||||
}
|
||||
|
||||
void Loweheiser::update()
|
||||
{
|
||||
// if (!_enabled.get()) {
|
||||
// return;
|
||||
// }
|
||||
maybe_send_heartbeat();
|
||||
update_receive();
|
||||
update_send();
|
||||
}
|
||||
|
||||
void Loweheiser::maybe_send_heartbeat()
|
||||
{
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
|
||||
if (now - last_heartbeat_ms < 100) {
|
||||
// we only provide a heartbeat every so often
|
||||
return;
|
||||
}
|
||||
last_heartbeat_ms = now;
|
||||
|
||||
mavlink_message_t msg;
|
||||
mavlink_msg_heartbeat_pack(system_id,
|
||||
component_id,
|
||||
&msg,
|
||||
MAV_TYPE_GCS,
|
||||
MAV_AUTOPILOT_INVALID,
|
||||
0,
|
||||
0,
|
||||
0);
|
||||
|
||||
uint8_t buf[300];
|
||||
uint16_t buf_len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
|
||||
if (write_to_autopilot((const char*)&buf, buf_len) != buf_len) {
|
||||
// ::fprintf(stderr, "write failure\n");
|
||||
}
|
||||
}
|
||||
|
||||
void Loweheiser::handle_message(const mavlink_message_t &msg)
|
||||
{
|
||||
switch (msg.msgid) {
|
||||
case MAVLINK_MSG_ID_COMMAND_LONG: {
|
||||
mavlink_command_long_t pkt;
|
||||
mavlink_msg_command_long_decode(&msg, &pkt);
|
||||
|
||||
if (pkt.target_system != system_id ||
|
||||
pkt.target_component != component_id) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Not for me");
|
||||
return;
|
||||
}
|
||||
|
||||
switch (pkt.command) {
|
||||
case MAV_CMD_LOWEHEISER_SET_STATE:
|
||||
// decode the desired run state:
|
||||
// param2 physically turns power on/off to the EFI!
|
||||
switch ((uint8_t)pkt.param2) {
|
||||
case 0:
|
||||
autopilot_desired_engine_run_state = EngineRunState::OFF;
|
||||
break;
|
||||
case 1:
|
||||
autopilot_desired_engine_run_state = EngineRunState::ON;
|
||||
break;
|
||||
default:
|
||||
AP_HAL::panic("Bad desired engine run state");
|
||||
}
|
||||
switch ((uint8_t)pkt.param3) {
|
||||
case 0:
|
||||
autopilot_desired_governor_state = GovernorState::OFF;
|
||||
break;
|
||||
case 1:
|
||||
autopilot_desired_governor_state = GovernorState::ON;
|
||||
break;
|
||||
default:
|
||||
AP_HAL::panic("Bad desired governor state");
|
||||
}
|
||||
switch ((uint8_t)pkt.param5) {
|
||||
case 0:
|
||||
autopilot_desired_startup_state = StartupState::OFF;
|
||||
break;
|
||||
case 1:
|
||||
autopilot_desired_startup_state = StartupState::ON;
|
||||
break;
|
||||
default:
|
||||
AP_HAL::panic("Bad electronic startup state");
|
||||
}
|
||||
manual_throttle_pct = pkt.param4;
|
||||
mavlink_message_t ack;
|
||||
mavlink_msg_command_ack_pack(
|
||||
system_id,
|
||||
component_id,
|
||||
&ack,
|
||||
MAV_CMD_LOWEHEISER_SET_STATE,
|
||||
MAV_RESULT_ACCEPTED,
|
||||
0,
|
||||
0,
|
||||
msg.sysid,
|
||||
msg.compid
|
||||
);
|
||||
uint8_t buf[300];
|
||||
uint16_t buf_len = mavlink_msg_to_send_buffer(buf, &ack);
|
||||
|
||||
if (write_to_autopilot((const char*)&buf, buf_len) != buf_len) {
|
||||
// ::fprintf(stderr, "write failure\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
void Loweheiser::update_receive()
|
||||
{
|
||||
char buf[128];
|
||||
const ssize_t bytes_read = read_from_autopilot(buf, sizeof(buf));
|
||||
if (bytes_read <= 0) {
|
||||
return;
|
||||
}
|
||||
for (uint16_t i=0; i<bytes_read; i++) {
|
||||
if (mavlink_frame_char_buffer(
|
||||
&rxmsg,
|
||||
&rxstatus,
|
||||
buf[i],
|
||||
&rxmsg,
|
||||
&rxstatus) == MAVLINK_FRAMING_OK) {
|
||||
handle_message(rxmsg);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Loweheiser::update_fuel_level()
|
||||
{
|
||||
const uint32_t now_ms = AP_HAL::millis();
|
||||
|
||||
const uint32_t tdelta = now_ms - last_fuel_update_ms;
|
||||
last_fuel_update_ms = now_ms;
|
||||
|
||||
const float fuel_consumption_lperh_at_8000rpm = 10;
|
||||
const float fuel_consumption_lpers_at_8000rpm = fuel_consumption_lperh_at_8000rpm / 3600;
|
||||
|
||||
fuel_flow_lps = generatorengine.current_rpm/8000 * fuel_consumption_lpers_at_8000rpm;
|
||||
const float fuel_delta = tdelta/1000.0 * fuel_flow_lps; // litres
|
||||
fuel_consumed += fuel_delta;
|
||||
fuel_level -= fuel_delta;
|
||||
}
|
||||
|
||||
void Loweheiser::update_send()
|
||||
{
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
if (now - last_sent_ms < 200) {
|
||||
return;
|
||||
}
|
||||
last_sent_ms = now;
|
||||
|
||||
auto sitl = AP::sitl();
|
||||
if (!sitl || sitl->efi_type == SIM::EFIType::EFI_TYPE_NONE) {
|
||||
return;
|
||||
}
|
||||
|
||||
float throttle = 0;
|
||||
float throttle_output = 0;
|
||||
|
||||
switch (autopilot_desired_engine_run_state) {
|
||||
case EngineRunState::OFF:
|
||||
generatorengine.desired_rpm = 0;
|
||||
break;
|
||||
case EngineRunState::ON:
|
||||
switch (autopilot_desired_governor_state) {
|
||||
case GovernorState::OFF: {
|
||||
throttle = manual_throttle_pct;
|
||||
throttle_output = throttle;
|
||||
if (is_positive(generatorengine.desired_rpm) ||
|
||||
autopilot_desired_startup_state == StartupState::ON) {
|
||||
const uint16_t manual_rpm_min = 0;
|
||||
const uint16_t manual_rpm_max = 8000;
|
||||
generatorengine.desired_rpm = linear_interpolate(
|
||||
manual_rpm_min,
|
||||
manual_rpm_max,
|
||||
throttle,
|
||||
0,
|
||||
100
|
||||
);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case GovernorState::ON:
|
||||
// should probably base this on current draw from battery
|
||||
// / motor throttle output?
|
||||
throttle = 80;
|
||||
throttle_output = 80;
|
||||
generatorengine.desired_rpm = 8000;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// a simulation for a stuck throttle - once the egine starts it
|
||||
// won't stop based on servo position,, only engine run state
|
||||
const bool stuck_throttle_failure_simulation = false;
|
||||
static bool throttle_is_stuck;
|
||||
if (stuck_throttle_failure_simulation) {
|
||||
if (generatorengine.desired_rpm > 7000) {
|
||||
throttle_is_stuck = true;
|
||||
}
|
||||
// if the throttle is stuck then the engine runs - except if
|
||||
// the autopilot is saying the desired runstate is off,
|
||||
// because that just shuts down the spark entirely, so the
|
||||
// engine will not run
|
||||
if (throttle_is_stuck &&
|
||||
autopilot_desired_engine_run_state == EngineRunState::ON) {
|
||||
// same numbers as "on" case, above
|
||||
throttle = 80;
|
||||
throttle_output = 80;
|
||||
generatorengine.desired_rpm = 8000;
|
||||
}
|
||||
}
|
||||
|
||||
_current_current = AP::sitl()->state.battery_current;
|
||||
_current_current = MIN(_current_current, max_current);
|
||||
|
||||
generatorengine.current_current = _current_current;
|
||||
generatorengine.max_current = max_current;
|
||||
generatorengine.max_slew_rpm_per_second = 2000;
|
||||
|
||||
generatorengine.update();
|
||||
|
||||
update_fuel_level();
|
||||
|
||||
float efi_pw = std::numeric_limits<float>::quiet_NaN();
|
||||
float efi_fuel_flow = std::numeric_limits<float>::quiet_NaN();
|
||||
float efi_fuel_consumed = std::numeric_limits<float>::quiet_NaN();
|
||||
float efi_fuel_level = std::numeric_limits<float>::quiet_NaN();
|
||||
float efi_baro = std::numeric_limits<float>::quiet_NaN();
|
||||
float efi_mat = std::numeric_limits<float>::quiet_NaN();
|
||||
float efi_clt = std::numeric_limits<float>::quiet_NaN();
|
||||
float efi_tps = std::numeric_limits<float>::quiet_NaN();
|
||||
float efi_egt = std::numeric_limits<float>::quiet_NaN();
|
||||
float efi_batt = std::numeric_limits<float>::quiet_NaN();
|
||||
|
||||
float curr_batt = -0.3;
|
||||
float curr_gen = 10.12;
|
||||
|
||||
// Current from the generator is the battery charging current (defined to be negative) plus the generator current
|
||||
float curr_rot = (curr_batt < 0 ? -curr_batt : 0.0) + curr_gen;
|
||||
|
||||
// controlled by param2, this turns on/off the DC/DC component which
|
||||
// powers the efi
|
||||
if (autopilot_desired_engine_run_state == EngineRunState::ON) {
|
||||
efi_baro = AP::baro().get_pressure() / 1000.0;
|
||||
efi_mat = AP::baro().get_temperature();
|
||||
efi_clt = generatorengine.temperature;
|
||||
efi_tps = MAX(throttle_output, 40);
|
||||
efi_batt = 12.5;
|
||||
efi_fuel_flow = fuel_flow_lps * 3600; // litres/second -> litres/hour
|
||||
}
|
||||
|
||||
if (false) {
|
||||
efi_fuel_level = fuel_level;
|
||||
efi_fuel_consumed = fuel_consumed;
|
||||
}
|
||||
|
||||
// +/- 3V, depending on draw
|
||||
const float volt_batt = base_supply_voltage - (3 * (_current_current / max_current));
|
||||
|
||||
const mavlink_loweheiser_gov_efi_t loweheiser_efi_gov{
|
||||
volt_batt, // volt_batt
|
||||
curr_batt, // curr_batt
|
||||
curr_gen, // curr_gen
|
||||
curr_rot, // curr_rot
|
||||
efi_fuel_level, // fuel_level in litres
|
||||
throttle, // throttle
|
||||
UINT32_MAX, // runtime in seconds
|
||||
INT32_MAX, // time until maintenance
|
||||
std::numeric_limits<float>::quiet_NaN(), // rectifier temperature
|
||||
std::numeric_limits<float>::quiet_NaN(), // generator temperature
|
||||
efi_batt, // efi_batt
|
||||
generatorengine.current_rpm, // efi_rpm
|
||||
efi_pw, // efi_pw
|
||||
efi_fuel_flow, // efi_fuelflow
|
||||
efi_fuel_consumed, // efi_fuel_consumed
|
||||
efi_baro, // efi_baro
|
||||
efi_mat, // efi_mat
|
||||
efi_clt, // efi_clt
|
||||
efi_tps, // efi_tps
|
||||
efi_egt, // efi_exhaust_gas_temperature
|
||||
1, // EFI index
|
||||
0, // generator_status
|
||||
0 // EFI status
|
||||
};
|
||||
|
||||
mavlink_message_t msg;
|
||||
mavlink_msg_loweheiser_gov_efi_encode_status(
|
||||
system_id,
|
||||
component_id,
|
||||
&mav_status,
|
||||
&msg,
|
||||
&loweheiser_efi_gov
|
||||
);
|
||||
|
||||
uint8_t buf[300];
|
||||
uint16_t buf_len = mavlink_msg_to_send_buffer(buf, &msg);
|
||||
|
||||
if (write_to_autopilot((char*)buf, buf_len) != buf_len) {
|
||||
AP_HAL::panic("Failed to write to autopilot: %s", strerror(errno));
|
||||
}
|
||||
}
|
||||
|
||||
#endif // AP_SIM_LOWEHEISER_ENABLED
|
||||
|
|
@ -0,0 +1,180 @@
|
|||
/*
|
||||
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/>.
|
||||
*/
|
||||
/*
|
||||
Simulator for the Loweheiser EFI/generator
|
||||
|
||||
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:loweheiser --speedup=1 --console
|
||||
|
||||
param set SIM_EFI_TYPE 2
|
||||
param set SERIAL5_PROTOCOL 2
|
||||
param set GEN_TYPE 4
|
||||
param set EFI_TYPE 4
|
||||
param set BATT2_MONITOR 17 # generator (elec)
|
||||
param set BATT3_MONITOR 18 # generator (fuel level)
|
||||
param fetch
|
||||
|
||||
param set BATT3_CAPACITY 10000
|
||||
param set BATT3_LOW_MAH 1000
|
||||
param set BATT3_CRT_MAH 500
|
||||
param set BATT3_FS_LOW_ACT 2 # RTL
|
||||
param set BATT3_FS_CRT_ACT 1 # LAND
|
||||
param set BATT3_LOW_VOLT 0
|
||||
|
||||
param set RC9_OPTION 85 # generator control
|
||||
|
||||
param set GEN_IDLE_TH_H 40 # NOTE without this the engine never warms up past 36 deg C
|
||||
param set GEN_IDLE_TH 25
|
||||
|
||||
param set RC10_OPTION 212 # loweheiser manual throttle control
|
||||
param set RC10_DZ 20
|
||||
param set RC10_TRIM 1000
|
||||
param set RC10_MIN 1000
|
||||
param set RC10_MAX 2000
|
||||
|
||||
param set RC11_OPTION 109 # loweheiser starter channel
|
||||
|
||||
reboot
|
||||
|
||||
# for testing failsafes:
|
||||
param set BATT3_CAPACITY 200
|
||||
param set BATT3_LOW_MAH 100
|
||||
param set BATT3_CRT_MAH 50
|
||||
|
||||
# stream EFI_STATUS at 10Hz:
|
||||
long SET_MESSAGE_INTERVAL 225 100000
|
||||
|
||||
# run SITL against real generator:
|
||||
DEV=/dev/serial/by-id/usb-Prolific_Technology_Inc._USB-Serial_Controller-if00-port0
|
||||
./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=uart:$DEV:115200 --speedup=1 --console
|
||||
|
||||
# run generator test script against simulator:
|
||||
python ./libraries/AP_Generator/scripts/test-loweheiser.py tcp:localhost:5762
|
||||
|
||||
# use the generator test script to control the generator:
|
||||
./libraries/AP_Generator/scripts/test-loweheiser.py $DEV
|
||||
|
||||
# observe RPM
|
||||
|
||||
# observe remaining fuel:
|
||||
graph BATTERY_STATUS[2].battery_remaining
|
||||
graph BATTERY_STATUS[2].current_consumed
|
||||
|
||||
# autotest suite:
|
||||
|
||||
./Tools/autotest/autotest.py --gdb --debug build.Copter test.Copter.Loweheiser
|
||||
|
||||
# use a usb-ttl cable to connect directly to mavlink-speaking generator:
|
||||
DEV=/dev/serial/by-id/usb-Prolific_Technology_Inc._USB-Serial_Controller-if00-port0
|
||||
mavproxy.py --master $DEV --baud 115200
|
||||
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_LOWEHEISER_ENABLED
|
||||
|
||||
#include <AP_Param/AP_Param.h>
|
||||
|
||||
#include "SITL_Input.h"
|
||||
|
||||
#include "SIM_SerialDevice.h"
|
||||
#include "SIM_GeneratorEngine.h"
|
||||
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
namespace SITL {
|
||||
|
||||
class Loweheiser : public SerialDevice {
|
||||
public:
|
||||
|
||||
Loweheiser();
|
||||
|
||||
// update state
|
||||
void update();
|
||||
|
||||
private:
|
||||
|
||||
// TODO: make these parameters:
|
||||
const uint8_t system_id = 17;
|
||||
const uint8_t component_id = 18;
|
||||
|
||||
const float max_current = 50.0f;
|
||||
const float base_supply_voltage = 50.0;
|
||||
|
||||
// we share channels with the ArduPilot binary!
|
||||
// Beware: the mavlink rangefinder and other stuff shares this channel.
|
||||
const mavlink_channel_t mavlink_ch = (mavlink_channel_t)(MAVLINK_COMM_0+5);
|
||||
|
||||
class SIM *_sitl;
|
||||
|
||||
uint32_t last_sent_ms;
|
||||
|
||||
void update_receive();
|
||||
void update_send();
|
||||
|
||||
void maybe_send_heartbeat();
|
||||
uint32_t last_heartbeat_ms;
|
||||
|
||||
void handle_message(const mavlink_message_t &msg);
|
||||
|
||||
enum class EngineRunState : uint8_t {
|
||||
OFF = 0,
|
||||
ON = 1,
|
||||
};
|
||||
EngineRunState autopilot_desired_engine_run_state = EngineRunState::OFF;
|
||||
|
||||
enum class GovernorState : uint8_t {
|
||||
OFF = 0,
|
||||
ON = 1,
|
||||
};
|
||||
GovernorState autopilot_desired_governor_state = GovernorState::OFF;
|
||||
|
||||
float manual_throttle_pct;
|
||||
|
||||
enum class StartupState : uint8_t {
|
||||
OFF = 0,
|
||||
ON = 1,
|
||||
};
|
||||
StartupState autopilot_desired_startup_state = StartupState::OFF;
|
||||
|
||||
mavlink_message_t rxmsg;
|
||||
mavlink_status_t rxstatus;
|
||||
|
||||
SIM_GeneratorEngine generatorengine;
|
||||
|
||||
float _current_current;
|
||||
|
||||
// fuel
|
||||
const float initial_fuel_level = 10; // litres, must match battery setup
|
||||
float fuel_level = initial_fuel_level; // litres
|
||||
float fuel_consumed = 0; // litres
|
||||
float fuel_flow_lps = 0; // litres/second
|
||||
void update_fuel_level();
|
||||
|
||||
uint32_t last_fuel_update_ms;
|
||||
|
||||
mavlink_status_t mav_status;
|
||||
|
||||
// parameters
|
||||
// AP_Int8 _enabled;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // AP_SIM_LOWEHEISER_ENABLED
|
||||
|
|
@ -128,47 +128,29 @@ void RichenPower::update_control_pin(const struct sitl_input &input)
|
|||
// RICHENPOWER, 13:49
|
||||
// Idle RMP 4800 +-300, RUN RPM 13000 +- 1500
|
||||
|
||||
uint16_t desired_rpm = 0;
|
||||
switch (_state) {
|
||||
case State::STOP:
|
||||
desired_rpm = 0;
|
||||
generatorengine.desired_rpm = 0;
|
||||
break;
|
||||
case State::IDLE:
|
||||
case State::STOPPING:
|
||||
desired_rpm = 4800; // +/- 300
|
||||
generatorengine.desired_rpm = 4800; // +/- 300
|
||||
break;
|
||||
case State::RUN:
|
||||
desired_rpm = 13000; // +/- 1500
|
||||
generatorengine.desired_rpm = 13000; // +/- 1500
|
||||
break;
|
||||
}
|
||||
|
||||
_current_current = AP::sitl()->state.battery_current;
|
||||
_current_current = MIN(_current_current, max_current);
|
||||
if (_current_current > 1 && _state != State::RUN) {
|
||||
AP_HAL::panic("Generator stalled due to high current demand");
|
||||
} else if (_current_current > max_current) {
|
||||
AP_HAL::panic("Generator stalled due to high current demand (run)");
|
||||
}
|
||||
|
||||
// linear degradation in RPM up to maximum load
|
||||
if (desired_rpm) {
|
||||
desired_rpm -= 1500 * (_current_current/max_current);
|
||||
}
|
||||
generatorengine.current_current = _current_current;
|
||||
generatorengine.max_current = max_current;
|
||||
generatorengine.max_slew_rpm_per_second = 2000;
|
||||
|
||||
const float max_slew_rpm_per_second = 2000;
|
||||
const float max_slew_rpm = max_slew_rpm_per_second * ((now - last_rpm_update_ms) / 1000.0f);
|
||||
last_rpm_update_ms = now;
|
||||
const float rpm_delta = _current_rpm - desired_rpm;
|
||||
if (rpm_delta > 0) {
|
||||
_current_rpm -= MIN(max_slew_rpm, rpm_delta);
|
||||
} else {
|
||||
_current_rpm += MIN(max_slew_rpm, abs(rpm_delta));
|
||||
}
|
||||
|
||||
// if (!is_zero(rpm_delta)) {
|
||||
// ::fprintf(stderr, "richenpower pwm: %f\n", _current_rpm);
|
||||
// }
|
||||
generatorengine.update();
|
||||
|
||||
_current_rpm = generatorengine.current_rpm;
|
||||
}
|
||||
|
||||
void RichenPower::RichenUnion::update_checksum()
|
||||
|
|
|
|||
|
|
@ -45,6 +45,7 @@ arm throttle (denied because generator not running)
|
|||
#include "SITL_Input.h"
|
||||
|
||||
#include "SIM_SerialDevice.h"
|
||||
#include "SIM_GeneratorEngine.h"
|
||||
|
||||
namespace SITL {
|
||||
|
||||
|
|
@ -94,8 +95,6 @@ private:
|
|||
|
||||
float _current_current;
|
||||
|
||||
uint32_t last_rpm_update_ms;
|
||||
|
||||
enum class Errors {
|
||||
MaintenanceRequired = 0,
|
||||
};
|
||||
|
|
@ -136,6 +135,8 @@ private:
|
|||
|
||||
// time we were asked to stop;
|
||||
uint32_t stop_start_ms;
|
||||
|
||||
SIM_GeneratorEngine generatorengine;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
#include <GCS_MAVLink/GCS_config.h>
|
||||
|
||||
#ifndef HAL_SIM_ADSB_ENABLED
|
||||
#define HAL_SIM_ADSB_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
|
|
@ -22,6 +23,10 @@
|
|||
#define AP_SIM_IS31FL3195_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
||||
#ifndef AP_SIM_LOWEHEISER_ENABLED
|
||||
#define AP_SIM_LOWEHEISER_ENABLED AP_SIM_ENABLED && HAL_MAVLINK_BINDINGS_ENABLED
|
||||
#endif
|
||||
|
||||
#ifndef AP_SIM_SHIP_ENABLED
|
||||
#define AP_SIM_SHIP_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
#include "SIM_ToneAlarm.h"
|
||||
#include "SIM_EFI_MegaSquirt.h"
|
||||
#include "SIM_RichenPower.h"
|
||||
#include "SIM_Loweheiser.h"
|
||||
#include "SIM_FETtecOneWireESC.h"
|
||||
#include "SIM_IntelligentEnergy24.h"
|
||||
#include "SIM_Ship.h"
|
||||
|
|
@ -298,6 +299,7 @@ public:
|
|||
enum EFIType {
|
||||
EFI_TYPE_NONE = 0,
|
||||
EFI_TYPE_MS = 1,
|
||||
EFI_TYPE_LOWEHEISER = 2,
|
||||
EFI_TYPE_HIRTH = 8,
|
||||
};
|
||||
|
||||
|
|
@ -455,6 +457,9 @@ public:
|
|||
ToneAlarm tonealarm_sim;
|
||||
SIM_Precland precland_sim;
|
||||
RichenPower richenpower_sim;
|
||||
#if AP_SIM_LOWEHEISER_ENABLED
|
||||
Loweheiser loweheiser_sim;
|
||||
#endif
|
||||
IntelligentEnergy24 ie24_sim;
|
||||
FETtecOneWireESC fetteconewireesc_sim;
|
||||
#if AP_TEST_DRONECAN_DRIVERS
|
||||
|
|
|
|||
Loading…
Reference in New Issue