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 Volz
17 :
18 : TODO:
19 : - check that -128-to-127 thing vs -100-to-100
20 :
21 : */
22 :
23 : #include "SIM_config.h"
24 :
25 : #if AP_SIM_VOLZ_ENABLED
26 :
27 : #include <AP_HAL/AP_HAL.h>
28 :
29 : extern const AP_HAL::HAL& hal;
30 :
31 : #include <AP_Math/AP_Math.h>
32 :
33 : #include "SIM_Volz.h"
34 : #include "SITL.h"
35 : #include <AP_HAL/utility/sparse-endian.h>
36 : #include <AP_Logger/AP_Logger.h>
37 :
38 : #include "SIM_Aircraft.h"
39 :
40 : #include <errno.h>
41 :
42 : using namespace SITL;
43 :
44 : // table of user settable parameters
45 : const AP_Param::GroupInfo Volz::var_info[] = {
46 :
47 : // @Param: ENA
48 : // @DisplayName: Volz simulator enable/disable
49 : // @Description: Allows you to enable (1) or disable (0) the Volz simulator
50 : // @Values: 0:Disabled,1:Enabled
51 : // @User: Advanced
52 : AP_GROUPINFO("ENA", 1, Volz, _enabled, 0),
53 :
54 : // @Param: MASK
55 : // @DisplayName: Volz override mask
56 : // @Description: mask of servo output channels to override with values from Volz protocol. Note these are indexed from 0 - so channel 3 (usually throttle) has value 4 in this bitmask (1<<2).
57 : // @User: Advanced
58 : AP_GROUPINFO("MASK", 2, Volz, _output_mask, (1U<<0)|(1U<<1)|(1U<<3)),
59 :
60 : // @Param: FMASK
61 : // @DisplayName: Volz fail mask
62 : // @Description: fail servo at current position. Channel 1 is bit 0.
63 : // @User: Advanced
64 : AP_GROUPINFO("FMASK", 3, Volz, _failed_mask, 0),
65 :
66 : AP_GROUPEND
67 : };
68 :
69 : // this isn't in the header as inlcuding sparse_endian in the header
70 : // causes compilation issues with redefinition of bswap16
71 45675 : void Volz::Command::update_checksum()
72 : {
73 45675 : crc = htobe16(calculate_checksum());
74 45675 : }
75 :
76 2096 : Volz::Volz() : SerialDevice::SerialDevice()
77 : {
78 2096 : AP_Param::setup_object_defaults(this, var_info);
79 :
80 : // initialise IDs
81 35632 : for (uint8_t n=0; n<ARRAY_SIZE(servos); n++) {
82 33536 : auto &s = servos[n];
83 33536 : s.id = n+1; // really should parameterise this
84 33536 : s.pcb_temperature = 25;
85 33536 : s.motor_temperature = 25;
86 33536 : s.primary_current = 0.1;
87 33536 : s.secondary_current = 0.1;
88 33536 : s.primary_voltage = 1.1;
89 33536 : s.secondary_voltage = 1.2;
90 : }
91 2096 : }
92 :
93 216058 : void Volz::update_servos(const class Aircraft &aircraft)
94 : {
95 216058 : const float ambient_degC = aircraft.ambient_outside_temperature_degC();
96 216058 : const uint32_t now_us = AP_HAL::micros();
97 216058 : const uint32_t delta_t_us = now_us - last_servo_update_us;
98 216058 : if (delta_t_us < 1000) { // only update simulation at 1kHz
99 108031 : return;
100 : }
101 108027 : last_servo_update_us = now_us;
102 108027 : const float dt = delta_t_us / 1000000.0;
103 :
104 1836459 : for (auto &servo : servos) {
105 1728432 : const uint8_t idx = servo.id - 1;
106 1728432 : if (idx > ARRAY_SIZE(aircraft.servo_filter)) {
107 0 : continue;
108 : }
109 1728432 : servo.position = aircraft.servo_filter[idx].angle();
110 :
111 : // update primary current - proportional to airspeed and aileron deflection:
112 1728432 : float airspeed = aircraft.get_airspeed_pitot();
113 1728432 : servo.primary_current = abs(sinf(radians(servo.position))) * airspeed * 10;
114 :
115 : // update temperatures
116 : // servo.pcb_temperature += servo.secondary_current *0.01 * dt;
117 1728432 : servo.pcb_temperature -= (servo.pcb_temperature-ambient_degC) * 0.1 * dt;
118 :
119 : #if HAL_LOGGING_ENABLED
120 1728432 : log_Servo(servo, now_us);
121 : #endif // HAL_LOGGING_ENABLED
122 : }
123 : }
124 :
125 218651 : void Volz::update(const class Aircraft &aircraft)
126 : {
127 218651 : if (!_enabled.get()) {
128 2593 : return;
129 : }
130 :
131 216058 : update_servos(aircraft);
132 :
133 216058 : update_input();
134 : }
135 :
136 45675 : void Volz::consume_command()
137 : {
138 45675 : if (buflen < sizeof(Command)) {
139 0 : AP_HAL::panic("No command to consume");
140 : }
141 45675 : if (buflen > sizeof(Command)) {
142 0 : memmove(&u.buffer[0], &u.buffer[sizeof(Command)], buflen-sizeof(Command));
143 : }
144 45675 : buflen -= sizeof(Command);
145 45675 : }
146 :
147 261733 : bool Volz::shift_command_to_front_of_buffer()
148 : {
149 261733 : while (buflen >= sizeof(Command)) {
150 45675 : if (u.command.calculate_checksum() == be16toh(u.command.crc)) {
151 45675 : return true;
152 : }
153 : // AP_HAL::panic("Invalid input");
154 :
155 : // sync error; just shift everything by 1...
156 0 : memmove(&u.buffer[0], &u.buffer[1], buflen-1);
157 0 : buflen--;
158 : }
159 216058 : return false;
160 : }
161 :
162 45675 : void Volz::process_command(const Command &command)
163 : {
164 45675 : if (command.actuator_id >= ARRAY_SIZE(servos)+1) {
165 0 : AP_HAL::panic("Invalid actuator ID (%u) received", command.actuator_id);
166 : }
167 45675 : Servo &servo = servos[command.actuator_id-1];
168 :
169 45675 : Command response {};
170 45675 : response.actuator_id = command.actuator_id;
171 :
172 45675 : switch (command.command_id) {
173 39152 : case CommandId::SET_EXTENDED_POSITION: {
174 39152 : servo.desired_position = linear_interpolate(-1, 1, UINT16_VALUE(command.arg1, command.arg2), 0x80, 0x0F80);
175 39152 : response.command_id = CommandId::EXTENDED_POSITION_RESPONSE;
176 39152 : const uint16_t scaled_position = linear_interpolate(0x80, 0x0F80, servo.position, -1, 1);
177 39152 : response.arg1 = HIGHBYTE(scaled_position);
178 39152 : response.arg2 = LOWBYTE(scaled_position);
179 39152 : break;
180 : }
181 2173 : case CommandId::READ_TEMPERATURE:
182 : // nb. MAX(50) may not be correct.... unknown how true device performs
183 2173 : response.command_id = CommandId::TEMPERATURE_RESPONSE;
184 2173 : response.arg1 = MAX(servo.motor_temperature, -50.0) + 50.0;
185 2173 : response.arg2 = MAX(servo.pcb_temperature, -50.0) + 50.0;
186 2173 : break;
187 2175 : case CommandId::READ_CURRENT:
188 2175 : response.command_id = CommandId::CURRENT_RESPONSE;
189 2175 : response.arg1 = MIN(servo.primary_current * 50, 255); // realistic?
190 2175 : response.arg2 = MIN(servo.secondary_current * 50, 255);
191 2175 : break;
192 2175 : case CommandId::READ_VOLTAGE:
193 2175 : response.command_id = CommandId::VOLTAGE_RESPONSE;
194 2175 : response.arg1 = MIN(servo.primary_voltage * 5, 255);
195 2175 : response.arg2 = MIN(servo.secondary_voltage * 5, 255);
196 2175 : break;
197 0 : case CommandId::CURRENT_RESPONSE:
198 : case CommandId::VOLTAGE_RESPONSE:
199 : case CommandId::TEMPERATURE_RESPONSE:
200 : case CommandId::EXTENDED_POSITION_RESPONSE:
201 0 : AP_HAL::panic("Response 'command' %u received", (unsigned)command.command_id);
202 : break;
203 0 : default:
204 0 : AP_HAL::panic("Unknown command %u received", (unsigned)command.command_id);
205 : }
206 :
207 45675 : response.update_checksum();
208 :
209 45675 : if (write_to_autopilot((char*)&response, sizeof(response)) != sizeof(response)) {
210 0 : AP_HAL::panic("short write");
211 : }
212 45675 : }
213 :
214 216058 : void Volz::update_input()
215 : {
216 216058 : const ssize_t n = read_from_autopilot((char*)&u.buffer[buflen], ARRAY_SIZE(u.buffer) - buflen);
217 216058 : if (n < 0) {
218 : // TODO: do better here
219 0 : if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) {
220 0 : AP_HAL::panic("Failed to read from autopilot");
221 : }
222 : } else {
223 216058 : buflen += n;
224 : }
225 :
226 261733 : while (shift_command_to_front_of_buffer()) {
227 45675 : process_command(u.command);
228 45675 : consume_command();
229 : }
230 216058 : }
231 :
232 217236 : void Volz::update_sitl_input_pwm(struct sitl_input &input)
233 : {
234 : // overwrite the SITL input values passed through from
235 : // sitl_model->update_model with those we're receiving serially
236 3693012 : for (auto &servo : servos) {
237 3475776 : const uint8_t idx = servo.id - 1;
238 3475776 : if (idx > ARRAY_SIZE(input.servos)) {
239 : // silently ignore; input.servos is 12-long, we are
240 : // usually 16-long
241 0 : continue;
242 : }
243 3475776 : if ((_output_mask.get() & (1U<<idx)) == 0) {
244 2172360 : continue;
245 : }
246 1303416 : servo.failed = _failed_mask.get() & (1U<<idx);
247 1303416 : input.servos[idx] = servo.pwm();
248 : }
249 217236 : }
250 :
251 : #if HAL_LOGGING_ENABLED
252 1728432 : void Volz::log_Servo(const Volz::Servo &servo, const uint32_t now_us)
253 : {
254 : // @LoggerMessage: SMVZ
255 : // @Description: Simulated Volz servo information
256 : // @Field: TimeUS: Time since system startup
257 : // @Field: Id: Volz servo ID
258 : // @Field: Pos: Current Simulated Position
259 : // @Field: DesPos: Desired Simulated Position
260 : // @Field: V: simulated servo voltage
261 : // @Field: A: simulated servo current
262 : // @Field: PCBT: simulated PCB Temperature
263 : // @Field: MotT: simulated motor Temperature
264 1728432 : AP::logger().WriteStreaming(
265 : "SMVZ",
266 : "TimeUS," "Id," "Pos," "DesPos," "V," "A," "PCBT," "MotT" ,
267 : "s" "#" "%" "%" "v" "A" "O" "O" ,
268 : "F" "-" "0" "0" "0" "0" "0" "0" ,
269 : "Q" "B" "f" "f" "f" "f" "f" "f" ,
270 : now_us,
271 1728432 : servo.id - 1,
272 1728432 : servo.position * 100,
273 1728432 : servo.desired_position * 100,
274 1728432 : servo.primary_voltage,
275 1728432 : servo.primary_current,
276 1728432 : servo.pcb_temperature,
277 1728432 : servo.motor_temperature
278 : );
279 1728432 : }
280 : #endif // HAL_LOGGING_ENABLED
281 :
282 : #endif // AP_SIM_VOLZ_ENABLED
|
