123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128 |
- #include <AP_HAL/AP_HAL.h>
- #include "AP_RangeFinder_LightWareSerial.h"
- #include <AP_SerialManager/AP_SerialManager.h>
- #include <ctype.h>
- extern const AP_HAL::HAL& hal;
- #define LIGHTWARE_DIST_MAX_CM 10000
- #define LIGHTWARE_OUT_OF_RANGE_ADD_CM 100
- AP_RangeFinder_LightWareSerial::AP_RangeFinder_LightWareSerial(RangeFinder::RangeFinder_State &_state,
- AP_RangeFinder_Params &_params,
- uint8_t serial_instance) :
- AP_RangeFinder_Backend(_state, _params)
- {
- const AP_SerialManager &serial_manager = AP::serialmanager();
- uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
- if (uart != nullptr) {
- uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance));
- }
- }
- bool AP_RangeFinder_LightWareSerial::detect(uint8_t serial_instance)
- {
- return AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr;
- }
- bool AP_RangeFinder_LightWareSerial::get_reading(uint16_t &reading_cm)
- {
- if (uart == nullptr) {
- return false;
- }
- float sum = 0;
- uint16_t valid_count = 0;
- uint16_t invalid_count = 0;
-
- int16_t nbytes = uart->available();
- while (nbytes-- > 0) {
- char c = uart->read();
- if (c == '\r') {
- linebuf[linebuf_len] = 0;
- const float dist = (float)atof(linebuf);
- if (!is_negative(dist)) {
- sum += dist;
- valid_count++;
- } else {
- invalid_count++;
- }
- linebuf_len = 0;
- } else if (isdigit(c) || c == '.' || c == '-') {
- linebuf[linebuf_len++] = c;
- if (linebuf_len == sizeof(linebuf)) {
-
- linebuf_len = 0;
- }
- }
- }
- uint32_t now = AP_HAL::millis();
- if (last_init_ms == 0 ||
- (now - last_init_ms > 1000 &&
- now - state.last_reading_ms > 1000)) {
-
-
-
- uart->write("www\r\n");
- last_init_ms = now;
- } else {
- uart->write('d');
- }
-
- if (valid_count > 0) {
- reading_cm = 100 * sum / valid_count;
- return true;
- }
-
- if (invalid_count > 0) {
- reading_cm = MIN(MAX(LIGHTWARE_DIST_MAX_CM, max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM), UINT16_MAX);
- return true;
- }
-
- return false;
- }
- void AP_RangeFinder_LightWareSerial::update(void)
- {
- if (get_reading(state.distance_cm)) {
-
- state.last_reading_ms = AP_HAL::millis();
- update_status();
- } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
- set_status(RangeFinder::RangeFinder_NoData);
- }
- }
|