Line data Source code
1 : #include "AP_Mount_config.h"
2 :
3 : #if HAL_SOLO_GIMBAL_ENABLED
4 :
5 : #include <AP_HAL/AP_HAL.h>
6 : #include <AP_AHRS/AP_AHRS.h>
7 : #include "SoloGimbal.h"
8 :
9 : #include <stdio.h>
10 : #include <GCS_MAVLink/GCS.h>
11 : #include <AP_Logger/AP_Logger.h>
12 :
13 : extern const AP_HAL::HAL& hal;
14 :
15 3068 : bool SoloGimbal::present()
16 : {
17 3068 : if (_state != GIMBAL_STATE_NOT_PRESENT && AP_HAL::millis()-_last_report_msg_ms > 3000) {
18 : // gimbal went away
19 0 : _state = GIMBAL_STATE_NOT_PRESENT;
20 0 : return false;
21 : }
22 :
23 3068 : return _state != GIMBAL_STATE_NOT_PRESENT;
24 : }
25 :
26 3068 : bool SoloGimbal::aligned()
27 : {
28 3068 : return present() && _state == GIMBAL_STATE_PRESENT_RUNNING;
29 : }
30 :
31 34991 : gimbal_mode_t SoloGimbal::get_mode()
32 : {
33 34991 : const auto &_ahrs = AP::ahrs();
34 :
35 34991 : if ((_gimbalParams.initialized() && is_zero(_gimbalParams.get_K_rate())) || (_ahrs.get_rotation_body_to_ned().c.z < 0 && !(_lockedToBody || _calibrator.running()))) {
36 34979 : return GIMBAL_MODE_IDLE;
37 12 : } else if (!_ekf.getStatus()) {
38 12 : return GIMBAL_MODE_POS_HOLD;
39 0 : } else if (_calibrator.running() || _lockedToBody) {
40 0 : return GIMBAL_MODE_POS_HOLD_FF;
41 : } else {
42 0 : return GIMBAL_MODE_STABILIZE;
43 : }
44 : }
45 :
46 18195 : void SoloGimbal::receive_feedback(mavlink_channel_t chan, const mavlink_message_t &msg)
47 : {
48 : mavlink_gimbal_report_t report_msg;
49 18195 : mavlink_msg_gimbal_report_decode(&msg, &report_msg);
50 18195 : uint32_t tnow_ms = AP_HAL::millis();
51 18195 : _last_report_msg_ms = tnow_ms;
52 :
53 18195 : _gimbalParams.set_channel(chan);
54 :
55 18195 : if (report_msg.target_system != 1) {
56 0 : _state = GIMBAL_STATE_NOT_PRESENT;
57 : } else {
58 18195 : GCS_MAVLINK::set_channel_private(chan);
59 : }
60 :
61 18195 : switch(_state) {
62 1 : case GIMBAL_STATE_NOT_PRESENT:
63 : // gimbal was just connected or we just rebooted, transition to PRESENT_INITIALIZING
64 1 : _gimbalParams.reset();
65 1 : _gimbalParams.set_param(GMB_PARAM_GMB_SYSID, 1);
66 1 : _state = GIMBAL_STATE_PRESENT_INITIALIZING;
67 1 : break;
68 :
69 12 : case GIMBAL_STATE_PRESENT_INITIALIZING:
70 12 : _gimbalParams.update();
71 12 : if (_gimbalParams.initialized()) {
72 : // parameters done initializing, finalize initialization and transition to aligning
73 1 : extract_feedback(report_msg);
74 1 : _ang_vel_mag_filt = 20;
75 1 : _filtered_joint_angles = _measurement.joint_angles;
76 1 : _vehicle_to_gimbal_quat_filt.from_vector312(_filtered_joint_angles.x,_filtered_joint_angles.y,_filtered_joint_angles.z);
77 1 : _ekf.reset();
78 1 : _state = GIMBAL_STATE_PRESENT_ALIGNING;
79 : }
80 12 : break;
81 :
82 1387 : case GIMBAL_STATE_PRESENT_ALIGNING:
83 1387 : _gimbalParams.update();
84 1387 : extract_feedback(report_msg);
85 1387 : update_estimators();
86 1387 : if (_ekf.getStatus()) {
87 : // EKF done aligning, transition to running
88 1 : _state = GIMBAL_STATE_PRESENT_RUNNING;
89 : }
90 1387 : break;
91 :
92 16795 : case GIMBAL_STATE_PRESENT_RUNNING:
93 16795 : _gimbalParams.update();
94 16795 : extract_feedback(report_msg);
95 16795 : update_estimators();
96 16795 : break;
97 : }
98 :
99 18195 : send_controls(chan);
100 18195 : }
101 :
102 18195 : void SoloGimbal::send_controls(mavlink_channel_t chan)
103 : {
104 18195 : if (_state == GIMBAL_STATE_PRESENT_RUNNING) {
105 : // get the gimbal quaternion estimate
106 16796 : Quaternion quatEst;
107 16796 : _ekf.getQuat(quatEst);
108 :
109 : // run rate controller
110 16796 : _ang_vel_dem_rads.zero();
111 16796 : switch(get_mode()) {
112 0 : case GIMBAL_MODE_POS_HOLD_FF: {
113 0 : _ang_vel_dem_rads += get_ang_vel_dem_body_lock();
114 0 : _ang_vel_dem_rads += get_ang_vel_dem_gyro_bias();
115 0 : float _ang_vel_dem_radsLen = _ang_vel_dem_rads.length();
116 0 : if (_ang_vel_dem_radsLen > radians(400)) {
117 0 : _ang_vel_dem_rads *= radians(400)/_ang_vel_dem_radsLen;
118 : }
119 0 : if (HAVE_PAYLOAD_SPACE(chan, GIMBAL_CONTROL)) {
120 0 : mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid,
121 : _ang_vel_dem_rads.x, _ang_vel_dem_rads.y, _ang_vel_dem_rads.z);
122 : }
123 0 : break;
124 : }
125 0 : case GIMBAL_MODE_STABILIZE: {
126 0 : _ang_vel_dem_rads += get_ang_vel_dem_yaw(quatEst);
127 0 : _ang_vel_dem_rads += get_ang_vel_dem_roll_tilt(quatEst);
128 0 : _ang_vel_dem_rads += get_ang_vel_dem_feedforward(quatEst);
129 0 : _ang_vel_dem_rads += get_ang_vel_dem_gyro_bias();
130 0 : float ang_vel_dem_norm = _ang_vel_dem_rads.length();
131 0 : if (ang_vel_dem_norm > radians(400)) {
132 0 : _ang_vel_dem_rads *= radians(400)/ang_vel_dem_norm;
133 : }
134 0 : if (HAVE_PAYLOAD_SPACE(chan, GIMBAL_CONTROL)) {
135 0 : mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid,
136 : _ang_vel_dem_rads.x, _ang_vel_dem_rads.y, _ang_vel_dem_rads.z);
137 : }
138 0 : break;
139 : }
140 16796 : default:
141 : case GIMBAL_MODE_IDLE:
142 : case GIMBAL_MODE_POS_HOLD:
143 16796 : break;
144 : }
145 : }
146 :
147 : // set GMB_POS_HOLD
148 18195 : if (get_mode() == GIMBAL_MODE_POS_HOLD) {
149 12 : _gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 1);
150 : } else {
151 18183 : _gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 0);
152 : }
153 :
154 : // set GMB_MAX_TORQUE
155 : float max_torque;
156 18195 : _gimbalParams.get_param(GMB_PARAM_GMB_MAX_TORQUE, max_torque, 0);
157 18195 : if (!is_equal(max_torque,_max_torque) && !is_zero(max_torque)) {
158 0 : _max_torque = max_torque;
159 : }
160 :
161 18195 : if (!hal.util->get_soft_armed() || joints_near_limits()) {
162 3795 : _gimbalParams.set_param(GMB_PARAM_GMB_MAX_TORQUE, _max_torque);
163 : } else {
164 14400 : _gimbalParams.set_param(GMB_PARAM_GMB_MAX_TORQUE, 0);
165 : }
166 18195 : }
167 :
168 18183 : void SoloGimbal::extract_feedback(const mavlink_gimbal_report_t& report_msg)
169 : {
170 18183 : _measurement.delta_time = report_msg.delta_time;
171 18183 : _measurement.delta_angles.x = report_msg.delta_angle_x;
172 18183 : _measurement.delta_angles.y = report_msg.delta_angle_y;
173 18183 : _measurement.delta_angles.z = report_msg.delta_angle_z;
174 18183 : _measurement.delta_velocity.x = report_msg.delta_velocity_x,
175 18183 : _measurement.delta_velocity.y = report_msg.delta_velocity_y;
176 18183 : _measurement.delta_velocity.z = report_msg.delta_velocity_z;
177 18183 : _measurement.joint_angles.x = report_msg.joint_roll;
178 18183 : _measurement.joint_angles.y = report_msg.joint_el;
179 18183 : _measurement.joint_angles.z = report_msg.joint_az;
180 :
181 18183 : if (_calibrator.get_status() == ACCEL_CAL_COLLECTING_SAMPLE) {
182 0 : _calibrator.new_sample(_measurement.delta_velocity,_measurement.delta_time);
183 : }
184 :
185 18183 : _measurement.delta_angles -= _gimbalParams.get_gyro_bias() * _measurement.delta_time;
186 18183 : _measurement.joint_angles -= _gimbalParams.get_joint_bias();
187 18183 : _measurement.delta_velocity -= _gimbalParams.get_accel_bias() * _measurement.delta_time;
188 18183 : Vector3f accel_gain = _gimbalParams.get_accel_gain();
189 18183 : _measurement.delta_velocity.x *= (is_zero(accel_gain.x) ? 1.0f : accel_gain.x);
190 18183 : _measurement.delta_velocity.y *= (is_zero(accel_gain.y) ? 1.0f : accel_gain.y);
191 18183 : _measurement.delta_velocity.z *= (is_zero(accel_gain.z) ? 1.0f : accel_gain.z);
192 :
193 : // update _ang_vel_mag_filt, used for accel sample readiness
194 18183 : Vector3f ang_vel = _measurement.delta_angles / _measurement.delta_time;
195 18183 : Vector3f ekf_gyro_bias;
196 18183 : _ekf.getGyroBias(ekf_gyro_bias);
197 18183 : ang_vel -= ekf_gyro_bias;
198 18183 : float alpha = constrain_float(_measurement.delta_time/(_measurement.delta_time+0.5f),0.0f,1.0f);
199 18183 : _ang_vel_mag_filt += (ang_vel.length()-_ang_vel_mag_filt)*alpha;
200 18183 : _ang_vel_mag_filt = MIN(_ang_vel_mag_filt,20.0f);
201 :
202 : // get complementary filter inputs
203 18183 : _vehicle_to_gimbal_quat.from_vector312(_measurement.joint_angles.x,_measurement.joint_angles.y,_measurement.joint_angles.z);
204 :
205 : // update log deltas
206 18183 : _log_dt += _measurement.delta_time;
207 18183 : _log_del_ang += _measurement.delta_angles;
208 18183 : _log_del_vel += _measurement.delta_velocity;
209 18183 : }
210 :
211 18182 : void SoloGimbal::update_estimators()
212 : {
213 18182 : if (_state == GIMBAL_STATE_NOT_PRESENT || _state == GIMBAL_STATE_PRESENT_INITIALIZING) {
214 0 : return;
215 : }
216 :
217 : // Run the gimbal attitude and gyro bias estimator
218 18182 : _ekf.RunEKF(_measurement.delta_time, _measurement.delta_angles, _measurement.delta_velocity, _measurement.joint_angles);
219 18182 : update_joint_angle_est();
220 : }
221 :
222 163676 : void SoloGimbal::readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
223 163676 : const AP_InertialSensor &ins = AP::ins();
224 :
225 163676 : if (ins_index < ins.get_gyro_count()) {
226 : float dAng_dt;
227 163676 : if (!ins.get_delta_angle(ins_index,dAng, dAng_dt)) {
228 0 : dAng = ins.get_gyro(ins_index) / ins.get_loop_rate_hz();
229 : }
230 : }
231 163676 : }
232 :
233 81838 : void SoloGimbal::update_fast() {
234 81838 : const AP_InertialSensor &ins = AP::ins();
235 :
236 : #if INS_MAX_INSTANCES > 1
237 81838 : if (ins.use_gyro(0) && ins.use_gyro(1)) {
238 : // dual gyro mode - average first two gyros
239 81838 : Vector3f dAng;
240 81838 : readVehicleDeltaAngle(0, dAng);
241 81838 : _vehicle_delta_angles += dAng*0.5f;
242 81838 : readVehicleDeltaAngle(1, dAng);
243 81838 : _vehicle_delta_angles += dAng*0.5f;
244 : } else
245 : #endif
246 : {
247 : // single gyro mode - one of the first two gyros are unhealthy or don't exist
248 : // just read primary gyro
249 0 : Vector3f dAng;
250 0 : readVehicleDeltaAngle(ins.get_first_usable_gyro(), dAng);
251 0 : _vehicle_delta_angles += dAng;
252 : }
253 81838 : }
254 :
255 18182 : void SoloGimbal::update_joint_angle_est()
256 : {
257 : static const float tc = 1.0f;
258 18182 : float dt = _measurement.delta_time;
259 18182 : float alpha = constrain_float(dt/(dt+tc),0.0f,1.0f);
260 :
261 18182 : Matrix3f Tvg; // vehicle frame to gimbal frame
262 18182 : _vehicle_to_gimbal_quat.inverse().rotation_matrix(Tvg);
263 :
264 18182 : Vector3f delta_angle_bias;
265 18182 : _ekf.getGyroBias(delta_angle_bias);
266 18182 : delta_angle_bias *= dt;
267 :
268 18182 : Vector3f joint_del_ang;
269 18182 : gimbal_ang_vel_to_joint_rates((_measurement.delta_angles-delta_angle_bias) - Tvg*_vehicle_delta_angles, joint_del_ang);
270 :
271 18182 : _filtered_joint_angles += joint_del_ang;
272 18182 : _filtered_joint_angles += (_measurement.joint_angles-_filtered_joint_angles)*alpha;
273 :
274 18182 : _vehicle_to_gimbal_quat_filt.from_vector312(_filtered_joint_angles.x,_filtered_joint_angles.y,_filtered_joint_angles.z);
275 :
276 18182 : _vehicle_delta_angles.zero();
277 18182 : }
278 :
279 0 : Vector3f SoloGimbal::get_ang_vel_dem_yaw(const Quaternion &quatEst)
280 : {
281 : static const float tc = 0.1f;
282 : static const float yawErrorLimit = radians(5.7f);
283 0 : float dt = _measurement.delta_time;
284 0 : float alpha = dt/(dt+tc);
285 :
286 0 : const auto &_ahrs = AP::ahrs();
287 0 : Matrix3f Tve = _ahrs.get_rotation_body_to_ned();
288 0 : Matrix3f Teg;
289 0 : quatEst.inverse().rotation_matrix(Teg);
290 :
291 :
292 : //_vehicle_yaw_rate_ef_filt = _ahrs.get_yaw_rate_earth();
293 :
294 : // filter the vehicle yaw rate to remove noise
295 0 : _vehicle_yaw_rate_ef_filt += (_ahrs.get_yaw_rate_earth() - _vehicle_yaw_rate_ef_filt) * alpha;
296 :
297 0 : float yaw_rate_ff = 0;
298 :
299 : // calculate an earth-frame yaw rate feed-forward that prevents gimbal from exceeding the maximum yaw error
300 0 : if (_vehicle_yaw_rate_ef_filt > _gimbalParams.get_K_rate()*yawErrorLimit) {
301 0 : yaw_rate_ff = _vehicle_yaw_rate_ef_filt-_gimbalParams.get_K_rate()*yawErrorLimit;
302 0 : } else if (_vehicle_yaw_rate_ef_filt < -_gimbalParams.get_K_rate()*yawErrorLimit) {
303 0 : yaw_rate_ff = _vehicle_yaw_rate_ef_filt+_gimbalParams.get_K_rate()*yawErrorLimit;
304 : }
305 :
306 : // filter the feed-forward to remove noise
307 : //_yaw_rate_ff_ef_filt += (yaw_rate_ff - _yaw_rate_ff_ef_filt) * alpha;
308 :
309 0 : Vector3f gimbalRateDemVecYaw;
310 0 : gimbalRateDemVecYaw.z = yaw_rate_ff - _gimbalParams.get_K_rate() * _filtered_joint_angles.z / constrain_float(Tve.c.z,0.5f,1.0f);
311 0 : gimbalRateDemVecYaw.z /= constrain_float(Tve.c.z,0.5f,1.0f);
312 :
313 : // rotate the rate demand into gimbal frame
314 0 : gimbalRateDemVecYaw = Teg * gimbalRateDemVecYaw;
315 :
316 0 : return gimbalRateDemVecYaw;
317 : }
318 :
319 0 : Vector3f SoloGimbal::get_ang_vel_dem_roll_tilt(const Quaternion &quatEst)
320 : {
321 : // Calculate the gimbal 321 Euler angle estimates relative to earth frame
322 0 : Vector3f eulerEst = quatEst.to_vector312();
323 :
324 : // Calculate a demanded quaternion using the demanded roll and pitch and estimated yaw (yaw is slaved to the vehicle)
325 0 : Quaternion quatDem;
326 0 : quatDem.from_vector312( _att_target_euler_rad.x,
327 : _att_target_euler_rad.y,
328 : eulerEst.z);
329 :
330 : //divide the demanded quaternion by the estimated to get the error
331 0 : Quaternion quatErr = quatDem / quatEst;
332 :
333 : // Convert to a delta rotation
334 0 : quatErr.normalize();
335 0 : Vector3f deltaAngErr;
336 0 : quatErr.to_axis_angle(deltaAngErr);
337 :
338 : // multiply the angle error vector by a gain to calculate a demanded gimbal rate required to control tilt
339 0 : Vector3f gimbalRateDemVecTilt = deltaAngErr * _gimbalParams.get_K_rate();
340 0 : return gimbalRateDemVecTilt;
341 : }
342 :
343 0 : Vector3f SoloGimbal::get_ang_vel_dem_feedforward(const Quaternion &quatEst) const
344 : {
345 : // quaternion demanded at the previous time step
346 : static float lastDemY;
347 :
348 : // calculate the delta rotation from the last to the current demand where the demand does not incorporate the copters yaw rotation
349 0 : float deltaY = _att_target_euler_rad.y - lastDemY;
350 0 : lastDemY = _att_target_euler_rad.y;
351 :
352 : // quaternion demanded at the previous time step
353 : static float lastDemX;
354 :
355 : // calculate the delta rotation from the last to the current demand where the demand does not incorporate the copters yaw rotation
356 0 : float deltaX = _att_target_euler_rad.x - lastDemX;
357 0 : lastDemX = _att_target_euler_rad.x;
358 :
359 0 : Vector3f gimbalRateDemVecForward;
360 0 : gimbalRateDemVecForward.x = deltaX / _measurement.delta_time;
361 0 : gimbalRateDemVecForward.y = deltaY / _measurement.delta_time;
362 0 : return gimbalRateDemVecForward;
363 : }
364 :
365 0 : Vector3f SoloGimbal::get_ang_vel_dem_gyro_bias()
366 : {
367 0 : Vector3f gyroBias;
368 0 : _ekf.getGyroBias(gyroBias);
369 0 : return gyroBias + _gimbalParams.get_gyro_bias();
370 : }
371 :
372 0 : Vector3f SoloGimbal::get_ang_vel_dem_body_lock()
373 : {
374 : // Define rotation from vehicle to gimbal using a 312 rotation sequence
375 0 : Matrix3f Tvg;
376 0 : _vehicle_to_gimbal_quat_filt.inverse().rotation_matrix(Tvg);
377 :
378 : // multiply the joint angles by a gain to calculate a rate vector required to keep the joints centred
379 0 : Vector3f gimbalRateDemVecBodyLock;
380 0 : gimbalRateDemVecBodyLock = _filtered_joint_angles * -_gimbalParams.get_K_rate();
381 :
382 0 : joint_rates_to_gimbal_ang_vel(gimbalRateDemVecBodyLock, gimbalRateDemVecBodyLock);
383 :
384 : // Add a feedforward term from vehicle gyros
385 0 : const auto &_ahrs = AP::ahrs();
386 0 : gimbalRateDemVecBodyLock += Tvg * _ahrs.get_gyro();
387 :
388 0 : return gimbalRateDemVecBodyLock;
389 : }
390 :
391 18195 : void SoloGimbal::update_target(const Vector3f &newTarget)
392 : {
393 : // Low-pass filter
394 18195 : _att_target_euler_rad.y = _att_target_euler_rad.y + 0.02f*(newTarget.y - _att_target_euler_rad.y);
395 : // Update tilt
396 18195 : _att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y,radians(-90),radians(0));
397 :
398 : // For roll angle
399 : // Low-pass filter
400 18195 : _att_target_euler_rad.x = _att_target_euler_rad.x + 0.02f*(newTarget.x - _att_target_euler_rad.x);
401 : // Update roll
402 18195 : _att_target_euler_rad.x = constrain_float(_att_target_euler_rad.x,radians(-30),radians(30));
403 18195 : }
404 :
405 : #if HAL_LOGGING_ENABLED
406 4545 : void SoloGimbal::write_logs()
407 : {
408 4545 : AP_Logger &logger = AP::logger();
409 :
410 4545 : const uint64_t tstamp = AP_HAL::micros64();
411 :
412 : // @LoggerMessage: GMB1
413 : // @Vehicles: Copter
414 : // @Description: Solo Gimbal measurements
415 : // @Field: TimeUS: Time since system startup
416 : // @Field: dt: sum of time across measurements in this packet
417 : // @Field: dax: delta-angle sum, x-axis
418 : // @Field: day: delta-angle sum, y-axis
419 : // @Field: daz: delta-angle sum, z-axis
420 : // @Field: dvx: delta-velocity sum, x-axis
421 : // @Field: dvy: delta-velocity sum, y-axis
422 : // @Field: dvz: delta-velocity sum, z-axis
423 : // @Field: jx: joint angle, x
424 : // @Field: jy: joint angle, y
425 : // @Field: jz: joint angle, z
426 4545 : logger.Write(
427 : "GMB1",
428 : "TimeUS,dt,dax,day,daz,dvx,dvy,dvz,jx,jy,jz",
429 : "ssrrrEEELLL",
430 : "FC000000000",
431 : "Qffffffffff",
432 : tstamp,
433 4545 : _log_dt,
434 4545 : _log_del_ang.x,
435 4545 : _log_del_ang.y,
436 4545 : _log_del_ang.z,
437 4545 : _log_del_vel.x,
438 4545 : _log_del_vel.y,
439 4545 : _log_del_vel.z,
440 4545 : _measurement.joint_angles.x,
441 4545 : _measurement.joint_angles.y,
442 4545 : _measurement.joint_angles.z
443 : );
444 :
445 4545 : Quaternion quatEst;
446 4545 : _ekf.getQuat(quatEst);
447 4545 : Vector3f eulerEst;
448 4545 : quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
449 :
450 : // @LoggerMessage: GMB2
451 : // @Vehicles: Copter
452 : // @Description: Solo Gimbal estimation and demands
453 : // @Field: TimeUS: Time since system startup
454 : // @Field: es: Solo Gimbal EKF status bits
455 : // @Field: ex: Solo Gimbal EKF estimate of gimbal angle, x-axis
456 : // @Field: ey: Solo Gimbal EKF estimate of gimbal angle, y-axis
457 : // @Field: ez: Solo Gimbal EKF estimate of gimbal angle, y-axis
458 : // @Field: rx: Angular velocity demand around x-axis
459 : // @Field: ry: Angular velocity demand around y-axis
460 : // @Field: rz: Angular velocity demand around z-axis
461 : // @Field: tx: Angular position target around x-axis
462 : // @Field: ty: Angular position target around y-axis
463 : // @Field: tz: Angular position target around z-axis
464 4545 : logger.Write(
465 : "GMB2",
466 : "TimeUS,es,ex,ey,ez,rx,ry,rz,tx,ty,tz",
467 : "s-rrrEEELLL",
468 : "F-000000000",
469 : "QBfffffffff",
470 : tstamp,
471 4545 : (uint8_t) _ekf.getStatus(),
472 4545 : eulerEst.x,
473 4545 : eulerEst.y,
474 4545 : eulerEst.z,
475 4545 : _ang_vel_dem_rads.x,
476 4545 : _ang_vel_dem_rads.y,
477 4545 : _ang_vel_dem_rads.z,
478 4545 : _att_target_euler_rad.x,
479 4545 : _att_target_euler_rad.y,
480 4545 : _att_target_euler_rad.z
481 : );
482 :
483 4545 : _log_dt = 0;
484 4545 : _log_del_ang.zero();
485 4545 : _log_del_vel.zero();
486 4545 : }
487 : #endif
488 :
489 14400 : bool SoloGimbal::joints_near_limits() const
490 : {
491 14400 : return fabsf(_measurement.joint_angles.x) > radians(40) || _measurement.joint_angles.y > radians(45) || _measurement.joint_angles.y < -radians(135);
492 : }
493 :
494 0 : AccelCalibrator* SoloGimbal::_acal_get_calibrator(uint8_t instance)
495 : {
496 0 : if(instance==0 && (present() || _calibrator.get_status() == ACCEL_CAL_SUCCESS)) {
497 0 : return &_calibrator;
498 : } else {
499 0 : return nullptr;
500 : }
501 : }
502 :
503 0 : bool SoloGimbal::_acal_get_ready_to_sample()
504 : {
505 0 : return _ang_vel_mag_filt < radians(10);
506 : }
507 :
508 0 : bool SoloGimbal::_acal_get_saving()
509 : {
510 0 : return _gimbalParams.flashing();
511 : }
512 :
513 0 : void SoloGimbal::_acal_save_calibrations()
514 : {
515 0 : if (_calibrator.get_status() != ACCEL_CAL_SUCCESS) {
516 0 : return;
517 : }
518 0 : Vector3f bias;
519 0 : Vector3f gain;
520 0 : _calibrator.get_calibration(bias,gain);
521 0 : _gimbalParams.set_accel_bias(bias);
522 0 : _gimbalParams.set_accel_gain(gain);
523 0 : _gimbalParams.flash();
524 : }
525 :
526 18182 : void SoloGimbal::gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates) const
527 : {
528 18182 : float sin_theta = sinf(_measurement.joint_angles.y);
529 18182 : float cos_theta = cosf(_measurement.joint_angles.y);
530 :
531 18182 : float sin_phi = sinf(_measurement.joint_angles.x);
532 18182 : float cos_phi = cosf(_measurement.joint_angles.x);
533 18182 : float sec_phi = 1.0f/cos_phi;
534 18182 : float tan_phi = sin_phi/cos_phi;
535 :
536 18182 : joint_rates.x = ang_vel.x*cos_theta+ang_vel.z*sin_theta;
537 18182 : joint_rates.y = ang_vel.x*sin_theta*tan_phi-ang_vel.z*cos_theta*tan_phi+ang_vel.y;
538 18182 : joint_rates.z = sec_phi*(ang_vel.z*cos_theta-ang_vel.x*sin_theta);
539 18182 : }
540 :
541 0 : void SoloGimbal::joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vector3f& ang_vel) const
542 : {
543 0 : float sin_theta = sinf(_measurement.joint_angles.y);
544 0 : float cos_theta = cosf(_measurement.joint_angles.y);
545 :
546 0 : float sin_phi = sinf(_measurement.joint_angles.x);
547 0 : float cos_phi = cosf(_measurement.joint_angles.x);
548 :
549 0 : ang_vel.x = cos_theta*joint_rates.x-sin_theta*cos_phi*joint_rates.z;
550 0 : ang_vel.y = joint_rates.y + sin_phi*joint_rates.z;
551 0 : ang_vel.z = sin_theta*joint_rates.x+cos_theta*cos_phi*joint_rates.z;
552 0 : }
553 :
554 : #endif // HAL_SOLO_GIMBAL_ENABLED
|
