AP_Mount: add topotek gimbal driver #27053
Conversation
Co-authored-by: Randy Mackay <[email protected]> changes from randy include display firmware version to user switch automatically to RC control simplfy set-gimbal-lock replace use of ctime use unix line endings comment improvements
| int8_t AP_Mount_Topotek::calculate_crc(int8_t *cmd, uint8_t len) | ||
| { | ||
| uint8_t crc = 0; | ||
| for (uint16_t i = 0; i<len; i++) { | ||
| crc += cmd[i]; | ||
| } | ||
| return(crc); | ||
| } | ||
|
|
||
| // hexadecimal to character conversion | ||
| int8_t AP_Mount_Topotek::hex2char(int8_t data) | ||
| { | ||
| if ((9 >= data)) { | ||
| return (data + '0'); | ||
| } else { | ||
| return (data - 10 + 'A'); | ||
| } | ||
| } | ||
|
|
||
| // character to number conversion | ||
| uint8_t AP_Mount_Topotek::char_to_number(int8_t data) | ||
| { | ||
| if (('0' <= data && '9' >= data)) { | ||
| return (data - '0'); | ||
| } | ||
| else if (('a' <= data) && ('z' >= data)) { | ||
| return (data - 'a' + 10); | ||
| } | ||
| else if (('A' <= data) && ('Z' >= data)) { | ||
| return (data - 'A' + 10); | ||
| } | ||
| return 0; | ||
| } | ||
|
|
There was a problem hiding this comment.
These methods are all available elsewhere in ArduPilot
| int8_t AP_Mount_Topotek::_set_pitch_angle_cmd[20] = "#tpUG6wGIP"; // set gimbal pitch angle command | ||
| int8_t AP_Mount_Topotek::_set_roll_angle_cmd[20] = "#tpUG6wGIR"; // set gimbal roll angle command | ||
| int8_t AP_Mount_Topotek::_set_yaw_pitch_roll_speed_cmd[20] = "#tpUG6wYPR"; // set the speed of gimbal yaw, pitch and roll command | ||
| int8_t AP_Mount_Topotek::_set_gimbal_time[45] = "#tpUDCwUTC"; // set the gimbal time command |
There was a problem hiding this comment.
why 45 here for a 10 character string?
| uint8_t _last_req_step; // 10hz request loop step (different requests are sent at various steps) | ||
| uint8_t _stop_order_count = 0; // number of stop commands sent since target rates became zero | ||
| float _measure_dist_m = -1.0f; // latest rangefinder distance (in meters) | ||
| int8_t _msg_buff[AP_MOUNT_TOPOTEK_PACKETLEN_MAX] {}; // buffer holding bytes from latest packet received. only used to calculate crc |
There was a problem hiding this comment.
why init this? just wastes flash
| bool _got_gimbal_basic_info; // whether the gimbal information is obtained | ||
| bool _last_zoom_stop = false; // record the latest zoom stopped state. | ||
| bool _last_focus_stop = false; // record the latest focus stopped state. | ||
| uint8_t _sent_time_count = 0; // count of current time messages sent to gimbal |
| int8_t _model_name[11] = ""; // gimbal type | ||
| ZoomType _zoom_type; // current zoom type | ||
| float _zoom_value; // current zoom value | ||
| uint32_t _firmware_ver = 0; // firmware version |
|
|
||
| // get the minute part of the latitude | ||
| double lat_min = (latitude - lat_deg) * 60.0; | ||
| sprintf((char*)_set_gimbal_lat + 10, "%c%02d%07.4f", lat > 0 ? 'N':'S', lat_deg, lat_min); |
There was a problem hiding this comment.
_set_gimbal_lat can just be a local. Similarly elsewhere
| double latitude = lat / 10000000.0; | ||
| double longitude = lng / 10000000.0; |
There was a problem hiding this comment.
Don't need to have those
| _set_yaw_angle_cmd[14] = hex2char((speed >> 4) & 0x0F); | ||
| _set_yaw_angle_cmd[15] = hex2char((speed) & 0x0F); | ||
|
|
||
| send_packet(_set_yaw_angle_cmd, 16); |
There was a problem hiding this comment.
maybe send_packet(const char *header, const char *fmt, ...), calls snprintf()
send_packet("#tpUG6wGIY", "%04x%02x", yaw_angle_cd, speed);
There was a problem hiding this comment.
The header could be a part of the fmt too.
|
We discussed this on the dev call and the major concern was that the driver consumes far too much flash (about 9K). Various devs have given suggestions on how it could be made smaller so I will attempt to rework the driver based on this advice. |
|
Okay, I will start working on the modifications, thank you. |
|
This came up on the dev call and Tridge and I agreed that we should help make this driver smaller. I've got a 2 week vacation coming up soon but hopefully we can help. |
Thank you for your help. I will try my best to complete it this week. Thank you. |
Sorry, I've been very busy recently. I will finish it as soon as possible |
This adds support for the Topotek gimbals and replaces #26666
This has been tested on real hardware including confirming that
Some of the changes compared to the initial PR are:
The only known issues are:
@laozhou-fujian this builds on your PR and the single commit is now Co-authored by both of us. Can you confirm you are OK with these changes?
Size comparison
