forked from ArduPilot/ardupilot
/
SIM_Loweheiser.h
174 lines (121 loc) · 4.38 KB
/
SIM_Loweheiser.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
/*
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;
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