123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445 |
- #include <AP_HAL/AP_HAL.h>
- #include "AP_Proximity_LightWareSF40C.h"
- #include <AP_SerialManager/AP_SerialManager.h>
- #include <ctype.h>
- #include <stdio.h>
- extern const AP_HAL::HAL& hal;
- AP_Proximity_LightWareSF40C::AP_Proximity_LightWareSF40C(AP_Proximity &_frontend,
- AP_Proximity::Proximity_State &_state,
- AP_SerialManager &serial_manager) :
- AP_Proximity_Backend(_frontend, _state)
- {
- uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0);
- if (uart != nullptr) {
- uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Lidar360, 0));
- }
- }
- bool AP_Proximity_LightWareSF40C::detect(AP_SerialManager &serial_manager)
- {
- return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0) != nullptr;
- }
- void AP_Proximity_LightWareSF40C::update(void)
- {
- if (uart == nullptr) {
- return;
- }
-
- bool initialised = initialise();
-
- check_for_reply();
-
- if (initialised) {
- request_new_data();
- }
-
- if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_SF40C_TIMEOUT_MS)) {
- set_status(AP_Proximity::Proximity_NoData);
- } else {
- set_status(AP_Proximity::Proximity_Good);
- }
- }
- float AP_Proximity_LightWareSF40C::distance_max() const
- {
- return 100.0f;
- }
- float AP_Proximity_LightWareSF40C::distance_min() const
- {
- return 0.20f;
- }
- bool AP_Proximity_LightWareSF40C::initialise()
- {
-
- if (_motor_direction > 1) {
- if ((_last_request_ms == 0) || AP_HAL::millis() - _last_request_ms > 1000) {
- set_motor_direction();
- }
- }
-
- if (_forward_direction != frontend.get_yaw_correction(state.instance)) {
- if ((_last_request_ms == 0) || AP_HAL::millis() - _last_request_ms > 1000) {
- set_forward_direction();
- }
- }
-
- if (_motor_speed == 0) {
- if ((_last_request_ms == 0) || AP_HAL::millis() - _last_request_ms > 1000) {
- set_motor_speed(true);
- }
- return false;
- }
-
- if (!_sector_initialised) {
- init_sectors();
- return false;
- }
- return true;
- }
- void AP_Proximity_LightWareSF40C::init_sectors()
- {
-
- uint8_t ignore_area_count = get_ignore_area_count();
- if (ignore_area_count == 0) {
- _sector_initialised = true;
- return;
- }
- uint8_t sector = 0;
- for (uint8_t i=0; i<ignore_area_count; i++) {
-
- uint16_t ign_area_angle;
- uint8_t ign_area_width;
- if (get_ignore_area(i, ign_area_angle, ign_area_width)) {
-
- int16_t start_angle, end_angle;
- get_next_ignore_start_or_end(1, ign_area_angle, start_angle);
- get_next_ignore_start_or_end(0, start_angle, end_angle);
- int16_t degrees_to_fill = wrap_360(end_angle - start_angle);
-
- while ((degrees_to_fill > 0) && (sector < PROXIMITY_SECTORS_MAX)) {
- uint16_t sector_size;
- if (degrees_to_fill >= 90) {
-
- sector_size = 45;
- } else if (degrees_to_fill > 45) {
-
- sector_size = degrees_to_fill / 2.0f;
- } else {
-
- sector_size = degrees_to_fill;
- }
-
- _sector_middle_deg[sector] = wrap_360(start_angle + sector_size / 2.0f);
- _sector_width_deg[sector] = sector_size;
-
- start_angle += sector_size;
- sector++;
- degrees_to_fill -= sector_size;
- }
- }
- }
-
- _num_sectors = sector;
-
- init_boundary();
-
- _sector_initialised = true;
- }
- void AP_Proximity_LightWareSF40C::set_motor_speed(bool on_off)
- {
-
- if (uart == nullptr) {
- return;
- }
-
- if (on_off) {
- uart->write("#MBS,3\r\n");
- } else {
- uart->write("#MBS,0\r\n");
- }
-
- uart->write("?MBS\r\n");
- _last_request_type = RequestType_MotorSpeed;
- _last_request_ms = AP_HAL::millis();
- }
- void AP_Proximity_LightWareSF40C::set_motor_direction()
- {
-
- if (uart == nullptr) {
- return;
- }
-
- if (frontend.get_orientation(state.instance) == 0) {
- uart->write("#MBD,0\r\n");
- } else {
- uart->write("#MBD,1\r\n");
- }
-
- uart->write("?MBD\r\n");
- _last_request_type = RequestType_MotorDirection;
- _last_request_ms = AP_HAL::millis();
- }
- void AP_Proximity_LightWareSF40C::set_forward_direction()
- {
-
- if (uart == nullptr) {
- return;
- }
-
- char request_str[15];
- int16_t yaw_corr = frontend.get_yaw_correction(state.instance);
- yaw_corr = constrain_int16(yaw_corr, -999, 999);
- snprintf(request_str, sizeof(request_str), "#MBF,%d\r\n", yaw_corr);
- uart->write(request_str);
-
- uart->write("?MBF\r\n");
- _last_request_type = RequestType_ForwardDirection;
- _last_request_ms = AP_HAL::millis();
- }
- void AP_Proximity_LightWareSF40C::request_new_data()
- {
- if (uart == nullptr) {
- return;
- }
-
- uint32_t now = AP_HAL::millis();
- if ((_last_request_type != RequestType_None) && ((now - _last_request_ms) > PROXIMITY_SF40C_TIMEOUT_MS)) {
- _last_request_type = RequestType_None;
- _last_request_ms = 0;
- }
-
- if (_last_request_type == RequestType_None) {
- _request_count++;
- if (_request_count >= 5) {
- send_request_for_health();
- _request_count = 0;
- } else {
-
- send_request_for_distance();
- }
- _last_request_ms = now;
- }
- }
- void AP_Proximity_LightWareSF40C::send_request_for_health()
- {
- if (uart == nullptr) {
- return;
- }
- uart->write("?GS\r\n");
- _last_request_type = RequestType_Health;
- _last_request_ms = AP_HAL::millis();
- }
- bool AP_Proximity_LightWareSF40C::send_request_for_distance()
- {
- if (uart == nullptr) {
- return false;
- }
-
- _last_sector++;
- if (_last_sector >= _num_sectors) {
- _last_sector = 0;
- }
-
- char request_str[16];
- snprintf(request_str, sizeof(request_str), "?TS,%u,%u\r\n",
- MIN(_sector_width_deg[_last_sector], 999),
- MIN(_sector_middle_deg[_last_sector], 999));
- uart->write(request_str);
-
- _last_request_type = RequestType_DistanceMeasurement;
- _last_request_ms = AP_HAL::millis();
- return true;
- }
- bool AP_Proximity_LightWareSF40C::check_for_reply()
- {
- if (uart == nullptr) {
- return false;
- }
-
-
-
-
-
-
- uint16_t count = 0;
- int16_t nbytes = uart->available();
- while (nbytes-- > 0) {
- char c = uart->read();
-
- if (c == '\r' || c == '\n') {
- if ((element_len[0] > 0)) {
- if (process_reply()) {
- count++;
- }
- }
-
- clear_buffers();
- ignore_reply = false;
- wait_for_space = false;
-
- } else if (c == '#' || ignore_reply) {
- ignore_reply = true;
-
- } else if (c == '?') {
- wait_for_space = true;
- } else if (wait_for_space) {
- if (c == ' ') {
- wait_for_space = false;
- }
-
- } else if (c == ',') {
- if ((element_num == 0) && (element_len[0] > 0)) {
- element_num++;
- } else {
-
- clear_buffers();
- ignore_reply = true;
- }
-
- } else if (isdigit(c) || c == '.' || c == '-') {
- element_buf[element_num][element_len[element_num]] = c;
- element_len[element_num]++;
- if (element_len[element_num] >= sizeof(element_buf[element_num])-1) {
-
- clear_buffers();
- ignore_reply = true;
- }
- }
- }
- return (count > 0);
- }
- bool AP_Proximity_LightWareSF40C::process_reply()
- {
- if (uart == nullptr) {
- return false;
- }
- bool success = false;
- switch (_last_request_type) {
- case RequestType_None:
- break;
- case RequestType_Health:
-
- if (element_len[0] > 0) {
- long int result = strtol(element_buf[0], nullptr, 16);
- if (result > 0) {
- _sensor_status.value = result;
- success = true;
- }
- }
- break;
- case RequestType_MotorSpeed:
- _motor_speed = atoi(element_buf[0]);
- success = true;
- break;
- case RequestType_MotorDirection:
- _motor_direction = atoi(element_buf[0]);
- success = true;
- break;
- case RequestType_ForwardDirection:
- _forward_direction = atoi(element_buf[0]);
- success = true;
- break;
- case RequestType_DistanceMeasurement:
- {
- float angle_deg = (float)atof(element_buf[0]);
- float distance_m = (float)atof(element_buf[1]);
- uint8_t sector;
- if (convert_angle_to_sector(angle_deg, sector)) {
- _angle[sector] = angle_deg;
- _distance[sector] = distance_m;
- _distance_valid[sector] = is_positive(distance_m);
- _last_distance_received_ms = AP_HAL::millis();
- success = true;
-
- update_boundary_for_sector(sector, true);
- }
- break;
- }
- default:
- break;
- }
-
- if (success) {
- _last_request_type = RequestType_None;
- }
- return success;
- }
- void AP_Proximity_LightWareSF40C::clear_buffers()
- {
- element_len[0] = 0;
- element_len[1] = 0;
- element_num = 0;
- memset(element_buf, 0, sizeof(element_buf));
- }
|