Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : Simulator for the RichenPower Hybrid generators
17 : */
18 :
19 : #include <AP_Math/AP_Math.h>
20 :
21 : #include "SIM_RichenPower.h"
22 : #include "SITL.h"
23 : #include <AP_HAL/utility/sparse-endian.h>
24 :
25 : #include <stdio.h>
26 : #include <errno.h>
27 :
28 : using namespace SITL;
29 :
30 : extern const AP_HAL::HAL& hal;
31 :
32 : // table of user settable parameters
33 : const AP_Param::GroupInfo RichenPower::var_info[] = {
34 :
35 : // @Param: ENABLE
36 : // @DisplayName: RichenPower Generator sim enable/disable
37 : // @Description: Allows you to enable (1) or disable (0) the RichenPower simulator
38 : // @Values: 0:Disabled,1:Enabled
39 : // @User: Advanced
40 : AP_GROUPINFO("ENABLE", 0, RichenPower, _enabled, 0),
41 :
42 : // @Param: CTRL
43 : // @DisplayName: Pin RichenPower is connectred to
44 : // @Description: The pin number that the RichenPower spinner servo is connected to. (start at 1)
45 : // @Range: 0 15
46 : // @User: Advanced
47 : AP_GROUPINFO("CTRL", 2, RichenPower, _ctrl_pin, -1),
48 :
49 : AP_GROUPEND
50 : };
51 :
52 2096 : RichenPower::RichenPower() : SerialDevice::SerialDevice()
53 : {
54 : ASSERT_STORAGE_SIZE(RichenPacket, 70);
55 :
56 2096 : AP_Param::setup_object_defaults(this, var_info);
57 :
58 2096 : u.packet.magic1 = 0xAA;
59 2096 : u.packet.magic2 = 0x55;
60 :
61 2096 : u.packet.version_major = 0x0A;
62 2096 : u.packet.version_minor = 0x00;
63 :
64 2096 : u.packet.footermagic1 = 0x55;
65 2096 : u.packet.footermagic2 = 0xAA;
66 2096 : }
67 :
68 6 : void RichenPower::set_run_state(State newstate) {
69 6 : hal.console->printf("Moving to state %u from %u\n", (unsigned)newstate, (unsigned)_state);
70 6 : _state = newstate;
71 6 : }
72 :
73 113479 : void RichenPower::update(const struct sitl_input &input)
74 : {
75 113479 : if (!_enabled.get()) {
76 1468 : return;
77 : }
78 112011 : update_control_pin(input);
79 112011 : update_send();
80 : }
81 :
82 112011 : void RichenPower::update_control_pin(const struct sitl_input &input)
83 : {
84 112011 : const uint32_t now = AP_HAL::millis();
85 :
86 : static const uint16_t INPUT_SERVO_PWM_STOP = 1200;
87 : static const uint16_t INPUT_SERVO_PWM_IDLE = 1500;
88 : // static const uint16_t INPUT_SERVO_PWM_RUN = 1900;
89 :
90 : // RICHENPOWER, 13:47
91 : // 1100~1300 for engine stop, 1300~1800 for idle, and 1800~2000 for run
92 :
93 112011 : const uint16_t control_pwm = _ctrl_pin >= 1 ? input.servos[_ctrl_pin-1] : -1;
94 112011 : if (_state != State::STOPPING) {
95 76477 : if (control_pwm <= INPUT_SERVO_PWM_STOP && _state != State::STOP) {
96 1 : if (stop_start_ms == 0) {
97 1 : stop_start_ms = now;
98 : }
99 : } else {
100 76476 : stop_start_ms = 0;
101 : }
102 : }
103 : // ::fprintf(stderr, "stop_start_ms=%u\n", stop_start_ms);
104 : State newstate;
105 112011 : if (control_pwm <= INPUT_SERVO_PWM_STOP) {
106 : // stop
107 72668 : if (stop_start_ms == 0 || now - stop_start_ms > 30000) {
108 37134 : newstate = State::STOP;
109 : } else {
110 35534 : newstate = State::STOPPING;
111 : }
112 39343 : } else if (control_pwm <= INPUT_SERVO_PWM_IDLE) {
113 33974 : newstate = State::IDLE;
114 : } else {
115 5369 : newstate = State::RUN;
116 : }
117 112011 : if (newstate != _state) {
118 6 : set_run_state(newstate);
119 : } else {
120 112005 : if (_state == State::STOP) {
121 : // pass
122 : } else {
123 74871 : _runtime_ms += now - _last_runtime_ms;
124 : }
125 112005 : _last_runtime_ms = now;
126 : }
127 :
128 : // RICHENPOWER, 13:49
129 : // Idle RMP 4800 +-300, RUN RPM 13000 +- 1500
130 :
131 112011 : switch (_state) {
132 37134 : case State::STOP:
133 37134 : generatorengine.desired_rpm = 0;
134 37134 : break;
135 69508 : case State::IDLE:
136 : case State::STOPPING:
137 69508 : generatorengine.desired_rpm = 4800; // +/- 300
138 69508 : break;
139 5369 : case State::RUN:
140 5369 : generatorengine.desired_rpm = 13000; // +/- 1500
141 5369 : break;
142 : }
143 :
144 112011 : _current_current = AP::sitl()->state.battery_current;
145 112011 : _current_current = MIN(_current_current, max_current);
146 :
147 112011 : generatorengine.current_current = _current_current;
148 112011 : generatorengine.max_current = max_current;
149 112011 : generatorengine.max_slew_rpm_per_second = 2000;
150 :
151 112011 : generatorengine.update();
152 :
153 112011 : _current_rpm = generatorengine.current_rpm;
154 112011 : }
155 :
156 93 : void RichenPower::RichenUnion::update_checksum()
157 : {
158 93 : packet.checksum = 0;
159 558 : for (uint8_t i=1; i<6; i++) {
160 465 : packet.checksum += htobe16(checksum_buffer[i]);
161 : }
162 93 : packet.checksum = htobe16(packet.checksum);
163 93 : }
164 :
165 112011 : void RichenPower::update_send()
166 : {
167 : // just send a chunk of data at 1Hz:
168 112011 : const uint32_t now = AP_HAL::millis();
169 112011 : if (now - last_sent_ms < 1000) {
170 111918 : return;
171 : }
172 93 : last_sent_ms = now;
173 :
174 93 : u.packet.rpm = htobe16(_current_rpm);
175 93 : uint32_t runtime_seconds_remainder = _runtime_ms / 1000;
176 93 : u.packet.runtime_hours = runtime_seconds_remainder / 3600;
177 93 : runtime_seconds_remainder %= 3600;
178 93 : u.packet.runtime_minutes = runtime_seconds_remainder / 60;
179 93 : runtime_seconds_remainder %= 60;
180 93 : u.packet.runtime_seconds = runtime_seconds_remainder;
181 :
182 93 : u.packet.runtime_seconds = runtime_seconds_remainder;
183 :
184 93 : const int32_t seconds_until_maintenance = (original_seconds_until_maintenance - _runtime_ms/1000.0f);
185 93 : uint16_t errors = htobe16(u.packet.errors);
186 93 : if (seconds_until_maintenance <= 0) {
187 0 : u.packet.seconds_until_maintenance = htobe32(0);
188 0 : errors |= (1U<<(uint8_t(Errors::MaintenanceRequired)));
189 : } else {
190 93 : u.packet.seconds_until_maintenance = htobe32(seconds_until_maintenance);
191 93 : errors &= ~(1U<<(uint8_t(Errors::MaintenanceRequired)));
192 : }
193 93 : u.packet.errors = htobe16(errors);
194 :
195 93 : switch (_state) {
196 34 : case State::IDLE:
197 : case State::RUN:
198 34 : u.packet.output_current = htobe16(_current_current * 100);
199 : // +/- 3V, depending on draw
200 34 : u.packet.output_voltage = htobe16(100*base_supply_voltage - 3 * (_current_current / max_current));
201 34 : break;
202 59 : case State::STOP:
203 : default:
204 59 : u.packet.output_current = 0;
205 59 : u.packet.output_voltage = 0;
206 59 : break;
207 : }
208 :
209 : enum class Mode {
210 : IDLE = 0,
211 : RUN = 1,
212 : CHARGE = 2,
213 : BALANCE = 3,
214 : OFF = 4,
215 : };
216 93 : switch (_state) {
217 30 : case State::STOP:
218 30 : u.packet.mode = (uint8_t)Mode::OFF;
219 30 : break;
220 59 : case State::STOPPING:
221 : case State::IDLE:
222 59 : u.packet.mode = (uint8_t)Mode::IDLE;
223 59 : break;
224 4 : case State::RUN:
225 4 : u.packet.mode = (uint8_t)Mode::RUN;
226 4 : break;
227 : }
228 :
229 93 : u.update_checksum();
230 :
231 : // const uint8_t data[] = {
232 : // 0xAA, 0x55, 0x00, 0x0A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x1E, 0xB0, 0x00, 0x10, 0x00, 0x00, 0x23, 0x7A, 0x23,
233 : // 0x7A, 0x11, 0x1D, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
234 : // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
235 : // 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1E, 0xBE, 0x55, 0xAA,
236 : // };
237 :
238 93 : if (write_to_autopilot((char*)u.parse_buffer, ARRAY_SIZE(u.parse_buffer)) != ARRAY_SIZE(u.parse_buffer)) {
239 0 : AP_HAL::panic("Failed to write to autopilot: %s", strerror(errno));
240 : }
241 : }
|
