Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include "AP_RangeFinder_BLPing.h"
      17             : 
      18             : #if AP_RANGEFINDER_BLPING_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : 
      22        1357 : void AP_RangeFinder_BLPing::update(void)
      23             : {
      24        1357 :     if (uart == nullptr) {
      25           0 :         return;
      26             :     }
      27        1357 :     AP_RangeFinder_Backend_Serial::update();
      28             : 
      29        1357 :     if (status() == RangeFinder::Status::NoData) {
      30           0 :         const uint32_t now = AP_HAL::millis();
      31             :         // initialise sensor if no distances recently
      32           0 :         if (now - last_init_ms > read_timeout_ms()) {
      33           0 :             last_init_ms = now;
      34           0 :             init_sensor();
      35             :         }
      36             :     }
      37             : }
      38             : 
      39           0 : void AP_RangeFinder_BLPing::init_sensor()
      40             : {
      41             :     // Set message interval between pings in ms
      42           0 :     uint16_t ping_interval = _sensor_rate_ms;
      43           0 :     protocol.send_message(uart, PingProtocol::MessageId::SET_PING_INTERVAL, reinterpret_cast<uint8_t*>(&ping_interval), sizeof(ping_interval));
      44             : 
      45             :     // Send a message requesting a continuous
      46           0 :     uint16_t continuous_message = static_cast<uint16_t>(PingProtocol::MessageId::DISTANCE_SIMPLE);
      47           0 :     protocol.send_message(uart, PingProtocol::MessageId::CONTINUOUS_START, reinterpret_cast<uint8_t*>(&continuous_message), sizeof(continuous_message));
      48           0 : }
      49             : 
      50             : // distance returned in reading_m, signal_ok is set to true if sensor reports a strong signal
      51        1357 : bool AP_RangeFinder_BLPing::get_reading(float &reading_m)
      52             : {
      53        1357 :     if (uart == nullptr) {
      54           0 :         return false;
      55             :     }
      56             : 
      57             :     struct {
      58             :         float sum_cm = 0;
      59             :         uint16_t count = 0;
      60         341 :         float mean() const { return sum_cm / count; };
      61        1357 :     } averageStruct;
      62             : 
      63             :     // read any available lines from the lidar
      64        6173 :     for (auto i=0; i<8192; i++) {
      65             :         uint8_t b;
      66        6173 :         if (!uart->read(b)) {
      67        1357 :             break;
      68             :         }
      69        4816 :         if (protocol.parse_byte(b) == PingProtocol::MessageId::DISTANCE_SIMPLE) {
      70         344 :             averageStruct.count++;
      71         344 :             averageStruct.sum_cm += protocol.get_distance_mm()*0.1f;
      72             :         }
      73             :     }
      74             : 
      75        1357 :     if (averageStruct.count > 0) {
      76             :         // return average distance of readings
      77         341 :         reading_m = averageStruct.mean() * 0.01f;
      78         341 :         return true;
      79             :     }
      80             : 
      81             :     // no readings so return false
      82        1016 :     return false;
      83             : }
      84             : 
      85         341 : int8_t AP_RangeFinder_BLPing::get_signal_quality_pct() const
      86             : {
      87         341 :     if (status() != RangeFinder::Status::Good) {
      88         249 :         return RangeFinder::SIGNAL_QUALITY_UNKNOWN;
      89             :     }
      90          92 :     return protocol.get_confidence();
      91             : }
      92             : 
      93          92 : uint8_t PingProtocol::get_confidence() const
      94             : {
      95          92 :     return msg.payload[4];
      96             : }
      97             : 
      98         344 : uint32_t PingProtocol::get_distance_mm() const
      99             : {
     100         344 :     return (uint32_t)msg.payload[0] |
     101         344 :             (uint32_t)msg.payload[1] << 8 |
     102         344 :             (uint32_t)msg.payload[2] << 16 |
     103         344 :             (uint32_t)msg.payload[3] << 24;
     104             : }
     105             : 
     106           0 : void PingProtocol::send_message(AP_HAL::UARTDriver *uart, PingProtocol::MessageId msg_id, const uint8_t *payload, uint16_t payload_len) const
     107             : {
     108           0 :     if (uart == nullptr) {
     109           0 :         return;
     110             :     }
     111             : 
     112             :     // check for sufficient space in outgoing buffer
     113           0 :     if (uart->txspace() < payload_len + 10U) {
     114           0 :         return;
     115             :     }
     116             : 
     117             :     // write header
     118           0 :     uart->write(_frame_header1);
     119           0 :     uart->write(_frame_header2);
     120           0 :     uint16_t crc = _frame_header1 + _frame_header2;
     121             : 
     122             :     // write payload length
     123           0 :     uart->write(LOWBYTE(payload_len));
     124           0 :     uart->write(HIGHBYTE(payload_len));
     125           0 :     crc += LOWBYTE(payload_len) + HIGHBYTE(payload_len);
     126             : 
     127             :     // message id
     128           0 :     uart->write(LOWBYTE(msg_id));
     129           0 :     uart->write(HIGHBYTE(msg_id));
     130           0 :     crc += LOWBYTE(msg_id) + HIGHBYTE(msg_id);
     131             : 
     132             :     // src dev id
     133           0 :     uart->write(_src_id);
     134           0 :     crc += _src_id;
     135             : 
     136             :     // destination dev id
     137           0 :     uart->write(_dst_id);
     138           0 :     crc += _dst_id;
     139             : 
     140             :     // payload
     141           0 :     if (payload != nullptr) {
     142           0 :         for (uint16_t i = 0; i<payload_len; i++) {
     143           0 :             uart->write(payload[i]);
     144           0 :             crc += payload[i];
     145             :         }
     146             :     }
     147             : 
     148             :     // checksum
     149           0 :     uart->write(LOWBYTE(crc));
     150           0 :     uart->write(HIGHBYTE(crc));
     151             : }
     152             : 
     153        4816 : PingProtocol::MessageId PingProtocol::parse_byte(uint8_t b)
     154             : {
     155             :     // process byte depending upon current state
     156        4816 :     switch (msg.state) {
     157             : 
     158         344 :     case ParserState::HEADER1:
     159         344 :         if (b == _frame_header1) {
     160         344 :             msg.crc_expected = _frame_header1;
     161         344 :             msg.state = ParserState::HEADER2;
     162         344 :             msg.done = false;
     163             :         }
     164         344 :         break;
     165             : 
     166         344 :     case ParserState::HEADER2:
     167         344 :         if (b == _frame_header2) {
     168         344 :             msg.crc_expected += _frame_header2;
     169         344 :             msg.state = ParserState::LEN_L;
     170             :         } else {
     171           0 :             msg.state = ParserState::HEADER1;
     172             :         }
     173         344 :         break;
     174             : 
     175         344 :     case ParserState::LEN_L:
     176         344 :         msg.payload_len = b;
     177         344 :         msg.crc_expected += b;
     178         344 :         msg.state = ParserState::LEN_H;
     179         344 :         break;
     180             : 
     181         344 :     case ParserState::LEN_H:
     182         344 :         msg.payload_len |= ((uint16_t)b << 8);
     183         344 :         msg.payload_recv = 0;
     184         344 :         msg.crc_expected += b;
     185         344 :         msg.state = ParserState::MSG_ID_L;
     186         344 :         break;
     187             : 
     188         344 :     case ParserState::MSG_ID_L:
     189         344 :         msg.id = b;
     190         344 :         msg.crc_expected += b;
     191         344 :         msg.state = ParserState::MSG_ID_H;
     192         344 :         break;
     193             : 
     194         344 :     case ParserState::MSG_ID_H:
     195         344 :         msg.id |= ((uint16_t)b << 8);
     196         344 :         msg.crc_expected += b;
     197         344 :         msg.state = ParserState::SRC_ID;
     198         344 :         break;
     199             : 
     200         344 :     case ParserState::SRC_ID:
     201         344 :         msg.crc_expected += b;
     202         344 :         msg.state = ParserState::DST_ID;
     203         344 :         break;
     204             : 
     205         344 :     case ParserState::DST_ID:
     206         344 :         msg.crc_expected += b;
     207         344 :         msg.state = ParserState::PAYLOAD;
     208         344 :         break;
     209             : 
     210        1376 :     case ParserState::PAYLOAD:
     211        1376 :         if (msg.payload_recv < msg.payload_len) {
     212        1376 :             if (msg.payload_recv < ARRAY_SIZE(msg.payload)) {
     213        1376 :                 msg.payload[msg.payload_recv] = b;
     214             :             }
     215        1376 :             msg.payload_recv++;
     216        1376 :             msg.crc_expected += b;
     217             :         }
     218        1376 :         if (msg.payload_recv == msg.payload_len) {
     219         344 :             msg.state = ParserState::CRC_L;
     220             :         }
     221        1376 :         break;
     222             : 
     223         344 :     case ParserState::CRC_L:
     224         344 :         msg.crc = b;
     225         344 :         msg.state = ParserState::CRC_H;
     226         344 :         break;
     227             : 
     228         344 :     case ParserState::CRC_H:
     229         344 :         msg.crc |= ((uint16_t)b << 8);
     230         344 :         msg.state = ParserState::HEADER1;
     231         344 :         msg.done = msg.crc_expected == msg.crc;
     232         344 :         break;
     233             :     }
     234             : 
     235             : 
     236        4816 :     return msg.done ? get_message_id() : MessageId::INVALID;
     237             : }
     238             : 
     239             : #endif  // AP_RANGEFINDER_BLPING_ENABLED