Line data Source code
1 : #include "AP_Mount_config.h"
2 :
3 : #if HAL_SOLO_GIMBAL_ENABLED
4 :
5 : #include "SoloGimbal_Parameters.h"
6 : #include <AP_HAL/AP_HAL.h>
7 : #include <AP_Logger/AP_Logger.h>
8 : #include <GCS_MAVLink/GCS.h>
9 : #include <stdio.h>
10 :
11 : extern const AP_HAL::HAL& hal;
12 :
13 : const uint32_t SoloGimbal_Parameters::_retry_period = 3000;
14 : const uint8_t SoloGimbal_Parameters::_max_fetch_attempts = 5;
15 :
16 1 : SoloGimbal_Parameters::SoloGimbal_Parameters()
17 : {
18 1 : reset();
19 1 : }
20 :
21 :
22 2 : void SoloGimbal_Parameters::reset()
23 : {
24 2 : memset(_params,0,sizeof(_params));
25 2 : _last_request_ms = 0;
26 2 : _last_set_ms = 0;
27 2 : _flashing_step = GMB_PARAM_NOT_FLASHING;
28 2 : }
29 :
30 299 : const char* SoloGimbal_Parameters::get_param_name(gmb_param_t param)
31 : {
32 299 : switch(param) {
33 26 : case GMB_PARAM_GMB_OFF_ACC_X:
34 26 : return "GMB_OFF_ACC_X";
35 25 : case GMB_PARAM_GMB_OFF_ACC_Y:
36 25 : return "GMB_OFF_ACC_Y";
37 24 : case GMB_PARAM_GMB_OFF_ACC_Z:
38 24 : return "GMB_OFF_ACC_Z";
39 23 : case GMB_PARAM_GMB_GN_ACC_X:
40 23 : return "GMB_GN_ACC_X";
41 22 : case GMB_PARAM_GMB_GN_ACC_Y:
42 22 : return "GMB_GN_ACC_Y";
43 21 : case GMB_PARAM_GMB_GN_ACC_Z:
44 21 : return "GMB_GN_ACC_Z";
45 20 : case GMB_PARAM_GMB_OFF_GYRO_X:
46 20 : return "GMB_OFF_GYRO_X";
47 19 : case GMB_PARAM_GMB_OFF_GYRO_Y:
48 19 : return "GMB_OFF_GYRO_Y";
49 18 : case GMB_PARAM_GMB_OFF_GYRO_Z:
50 18 : return "GMB_OFF_GYRO_Z";
51 17 : case GMB_PARAM_GMB_OFF_JNT_X:
52 17 : return "GMB_OFF_JNT_X";
53 16 : case GMB_PARAM_GMB_OFF_JNT_Y:
54 16 : return "GMB_OFF_JNT_Y";
55 15 : case GMB_PARAM_GMB_OFF_JNT_Z:
56 15 : return "GMB_OFF_JNT_Z";
57 14 : case GMB_PARAM_GMB_K_RATE:
58 14 : return "GMB_K_RATE";
59 15 : case GMB_PARAM_GMB_POS_HOLD:
60 15 : return "GMB_POS_HOLD";
61 15 : case GMB_PARAM_GMB_MAX_TORQUE:
62 15 : return "GMB_MAX_TORQUE";
63 4 : case GMB_PARAM_GMB_SND_TORQUE:
64 4 : return "GMB_SND_TORQUE";
65 4 : case GMB_PARAM_GMB_SYSID:
66 4 : return "GMB_SYSID";
67 1 : case GMB_PARAM_GMB_FLASH:
68 1 : return "GMB_FLASH";
69 0 : default:
70 0 : return "";
71 : };
72 : }
73 :
74 1 : void SoloGimbal_Parameters::fetch_params()
75 : {
76 19 : for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
77 18 : if (_params[i].state != GMB_PARAMSTATE_NOT_YET_READ) {
78 3 : _params[i].state = GMB_PARAMSTATE_FETCH_AGAIN;
79 : }
80 : }
81 1 : }
82 :
83 35003 : bool SoloGimbal_Parameters::initialized()
84 : {
85 664657 : for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
86 629677 : if(_params[i].state == GMB_PARAMSTATE_NOT_YET_READ) {
87 23 : return false;
88 : }
89 : }
90 34980 : return true;
91 : }
92 :
93 18194 : bool SoloGimbal_Parameters::received_all()
94 : {
95 345495 : for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
96 327312 : if(_params[i].state == GMB_PARAMSTATE_NOT_YET_READ || _params[i].state == GMB_PARAMSTATE_FETCH_AGAIN) {
97 11 : return false;
98 : }
99 : }
100 18183 : return true;
101 : }
102 :
103 271370 : void SoloGimbal_Parameters::get_param(gmb_param_t param, float& value, float def_val) {
104 271370 : if (!_params[param].seen) {
105 1 : value = def_val;
106 : } else {
107 271369 : value = _params[param].value;
108 : }
109 271370 : }
110 :
111 36391 : void SoloGimbal_Parameters::set_param(gmb_param_t param, float value) {
112 72776 : if ((_params[param].state == GMB_PARAMSTATE_CONSISTENT && param != GMB_PARAM_GMB_FLASH &&
113 109175 : is_equal(_params[param].value,value)) || _params[param].state == GMB_PARAMSTATE_NONEXISTANT ||
114 8 : !HAVE_PAYLOAD_SPACE(_chan, PARAM_SET)) {
115 36383 : return;
116 : }
117 :
118 8 : _params[param].state = GMB_PARAMSTATE_ATTEMPTING_TO_SET;
119 8 : _params[param].value = value;
120 :
121 : // make a temporary copy of the ID; mavlink_msg_param_set_send
122 : // expects an array of the full length
123 8 : char tmp_name[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN+1] {};
124 8 : strncpy(tmp_name, get_param_name(param), MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN);
125 :
126 8 : mavlink_msg_param_set_send(_chan, 0, MAV_COMP_ID_GIMBAL, tmp_name, _params[param].value, MAV_PARAM_TYPE_REAL32);
127 :
128 8 : _last_set_ms = AP_HAL::millis();
129 : }
130 :
131 18194 : void SoloGimbal_Parameters::update()
132 : {
133 18194 : uint32_t tnow_ms = AP_HAL::millis();
134 :
135 : // retry initial param retrieval
136 18195 : if(!received_all() && ((tnow_ms - _last_request_ms) > _retry_period) &&
137 1 : (HAVE_PAYLOAD_SPACE(_chan, PARAM_REQUEST_LIST))) {
138 1 : _last_request_ms = tnow_ms;
139 1 : mavlink_msg_param_request_list_send(_chan, 0, MAV_COMP_ID_GIMBAL);
140 :
141 19 : for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
142 18 : if (!_params[i].seen) {
143 15 : _params[i].fetch_attempts++;
144 : }
145 : }
146 : }
147 :
148 : // retry param_set
149 345686 : for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
150 327492 : if (!HAVE_PAYLOAD_SPACE(_chan, PARAM_SET)) {
151 0 : break;
152 : }
153 :
154 327492 : if ((_params[i].state == GMB_PARAMSTATE_ATTEMPTING_TO_SET) && (tnow_ms - _last_set_ms > _retry_period)) {
155 0 : mavlink_msg_param_set_send(_chan, 0, MAV_COMP_ID_GIMBAL, get_param_name((gmb_param_t)i), _params[i].value, MAV_PARAM_TYPE_REAL32);
156 0 : _last_set_ms = AP_HAL::millis();
157 :
158 0 : if (!_params[i].seen) {
159 0 : _params[i].fetch_attempts++;
160 : }
161 : }
162 : }
163 :
164 : // check for nonexistent parameters
165 345686 : for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
166 327492 : if (!_params[i].seen && _params[i].fetch_attempts > _max_fetch_attempts) {
167 0 : _params[i].state = GMB_PARAMSTATE_NONEXISTANT;
168 0 : DEV_PRINTF("Gimbal parameter %s timed out\n", get_param_name((gmb_param_t)i));
169 : }
170 : }
171 :
172 18194 : if(_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_SET) {
173 0 : bool done = true;
174 0 : for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
175 0 : if (_params[i].state == GMB_PARAMSTATE_ATTEMPTING_TO_SET) {
176 0 : done = false;
177 0 : break;
178 : }
179 : }
180 :
181 0 : if (done) {
182 0 : _flashing_step = GMB_PARAM_FLASHING_WAITING_FOR_ACK;
183 0 : set_param(GMB_PARAM_GMB_FLASH,69.0f);
184 : }
185 : }
186 18194 : }
187 :
188 26 : void SoloGimbal_Parameters::handle_param_value(const mavlink_message_t &msg)
189 : {
190 : mavlink_param_value_t packet;
191 26 : mavlink_msg_param_value_decode(&msg, &packet);
192 :
193 : #if HAL_LOGGING_ENABLED
194 26 : AP_Logger *logger = AP_Logger::get_singleton();
195 26 : if (logger != nullptr) {
196 26 : logger->Write_Parameter(packet.param_id, packet.param_value);
197 : }
198 : #endif
199 :
200 291 : for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) {
201 291 : if (!strcmp(packet.param_id, get_param_name((gmb_param_t)i))) {
202 26 : _params[i].seen = true;
203 26 : switch(_params[i].state) {
204 18 : case GMB_PARAMSTATE_NONEXISTANT:
205 : case GMB_PARAMSTATE_NOT_YET_READ:
206 : case GMB_PARAMSTATE_FETCH_AGAIN:
207 18 : _params[i].value = packet.param_value;
208 18 : _params[i].state = GMB_PARAMSTATE_CONSISTENT;
209 18 : break;
210 3 : case GMB_PARAMSTATE_CONSISTENT:
211 3 : _params[i].value = packet.param_value;
212 3 : break;
213 5 : case GMB_PARAMSTATE_ATTEMPTING_TO_SET:
214 5 : if (i == GMB_PARAM_GMB_FLASH) {
215 0 : if (_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_ACK && (int)packet.param_value == 1) {
216 0 : _flashing_step = GMB_PARAM_NOT_FLASHING;
217 : }
218 0 : _params[i].value = 0;
219 0 : _params[i].state = GMB_PARAMSTATE_CONSISTENT;
220 5 : } else if (is_equal(packet.param_value,_params[i].value)) {
221 5 : _params[i].state = GMB_PARAMSTATE_CONSISTENT;
222 : }
223 5 : break;
224 : }
225 26 : break;
226 : }
227 : }
228 26 : }
229 :
230 18183 : Vector3f SoloGimbal_Parameters::get_accel_bias()
231 : {
232 18183 : Vector3f ret;
233 18183 : get_param(GMB_PARAM_GMB_OFF_ACC_X,ret.x);
234 18183 : get_param(GMB_PARAM_GMB_OFF_ACC_Y,ret.y);
235 18183 : get_param(GMB_PARAM_GMB_OFF_ACC_Z,ret.z);
236 18183 : return ret;
237 : }
238 18183 : Vector3f SoloGimbal_Parameters::get_accel_gain()
239 : {
240 18183 : Vector3f ret;
241 18183 : get_param(GMB_PARAM_GMB_GN_ACC_X,ret.x,1.0f);
242 18183 : get_param(GMB_PARAM_GMB_GN_ACC_Y,ret.y,1.0f);
243 18183 : get_param(GMB_PARAM_GMB_GN_ACC_Z,ret.z,1.0f);
244 18183 : return ret;
245 : }
246 :
247 0 : void SoloGimbal_Parameters::set_accel_bias(const Vector3f& bias)
248 : {
249 0 : set_param(GMB_PARAM_GMB_OFF_ACC_X, bias.x);
250 0 : set_param(GMB_PARAM_GMB_OFF_ACC_Y, bias.y);
251 0 : set_param(GMB_PARAM_GMB_OFF_ACC_Z, bias.z);
252 0 : }
253 :
254 0 : void SoloGimbal_Parameters::set_accel_gain(const Vector3f& gain)
255 : {
256 0 : set_param(GMB_PARAM_GMB_GN_ACC_X, gain.x);
257 0 : set_param(GMB_PARAM_GMB_GN_ACC_Y, gain.y);
258 0 : set_param(GMB_PARAM_GMB_GN_ACC_Z, gain.z);
259 0 : }
260 :
261 18183 : Vector3f SoloGimbal_Parameters::get_gyro_bias()
262 : {
263 18183 : Vector3f ret;
264 18183 : get_param(GMB_PARAM_GMB_OFF_GYRO_X,ret.x);
265 18183 : get_param(GMB_PARAM_GMB_OFF_GYRO_Y,ret.y);
266 18183 : get_param(GMB_PARAM_GMB_OFF_GYRO_Z,ret.z);
267 18183 : return ret;
268 : }
269 :
270 0 : void SoloGimbal_Parameters::set_gyro_bias(const Vector3f& bias)
271 : {
272 0 : set_param(GMB_PARAM_GMB_OFF_GYRO_X,bias.x);
273 0 : set_param(GMB_PARAM_GMB_OFF_GYRO_Y,bias.y);
274 0 : set_param(GMB_PARAM_GMB_OFF_GYRO_Z,bias.z);
275 0 : }
276 :
277 18183 : Vector3f SoloGimbal_Parameters::get_joint_bias()
278 : {
279 18183 : Vector3f ret;
280 18183 : get_param(GMB_PARAM_GMB_OFF_JNT_X,ret.x);
281 18183 : get_param(GMB_PARAM_GMB_OFF_JNT_Y,ret.y);
282 18183 : get_param(GMB_PARAM_GMB_OFF_JNT_Z,ret.z);
283 18183 : return ret;
284 : }
285 34979 : float SoloGimbal_Parameters::get_K_rate()
286 : {
287 : float ret;
288 34979 : get_param(GMB_PARAM_GMB_K_RATE,ret);
289 34979 : return ret;
290 : }
291 :
292 0 : void SoloGimbal_Parameters::flash()
293 : {
294 0 : _flashing_step = GMB_PARAM_FLASHING_WAITING_FOR_SET;
295 0 : }
296 :
297 0 : bool SoloGimbal_Parameters::flashing()
298 : {
299 0 : return _flashing_step != GMB_PARAM_NOT_FLASHING;
300 : }
301 :
302 : #endif // HAL_SOLO_GIMBAL_ENABLED
|
