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             :    The Lightware SF45B serial interface is described on this wiki page
      16             :    http://support.lightware.co.za/sf45/#/commands
      17             :  */
      18             : 
      19             : #include "AP_Proximity_config.h"
      20             : 
      21             : #if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED
      22             : 
      23             : #include "AP_Proximity_LightWareSF45B.h"
      24             : 
      25             : #include <AP_Common/AP_Common.h>
      26             : #include <AP_HAL/AP_HAL.h>
      27             : #include <AP_HAL/utility/sparse-endian.h>
      28             : 
      29             : extern const AP_HAL::HAL& hal;
      30             : 
      31             : static const uint32_t PROXIMITY_SF45B_TIMEOUT_MS = 200;
      32             : static const uint32_t PROXIMITY_SF45B_REINIT_INTERVAL_MS = 5000;    // re-initialise sensor after this many milliseconds
      33             : static const float PROXIMITY_SF45B_COMBINE_READINGS_DEG = 5.0f;     // combine readings from within this many degrees to improve efficiency
      34             : static const uint32_t PROXIMITY_SF45B_STREAM_DISTANCE_DATA_CM = 5;
      35             : static const uint8_t PROXIMITY_SF45B_DESIRED_UPDATE_RATE = 6;       // 1:48hz, 2:55hz, 3:64hz, 4:77hz, 5:97hz, 6:129hz, 7:194hz, 8:388hz
      36             : static const uint32_t PROXIMITY_SF45B_DESIRED_FIELDS = ((uint32_t)1 << 0 | (uint32_t)1 << 8);   // first return (unfiltered), yaw angle
      37             : static const uint16_t PROXIMITY_SF45B_DESIRED_FIELD_COUNT = 2;      // DISTANCE_DATA_CM message should contain two fields
      38             : 
      39             : // update the state of the sensor
      40        8248 : void AP_Proximity_LightWareSF45B::update(void)
      41             : {
      42        8248 :     if (_uart == nullptr) {
      43           0 :         return;
      44             :     }
      45             : 
      46             :     // initialise sensor if necessary
      47        8248 :     initialise();
      48             : 
      49             :     // process incoming messages
      50        8248 :     process_replies();
      51             : 
      52             :     // check for timeout and set health status
      53        8248 :     if ((_last_distance_received_ms == 0) || ((AP_HAL::millis() - _last_distance_received_ms) > PROXIMITY_SF45B_TIMEOUT_MS)) {
      54         514 :         set_status(AP_Proximity::Status::NoData);
      55             :     } else {
      56        7734 :         set_status(AP_Proximity::Status::Good);
      57             :     }
      58             : }
      59             : 
      60             : // initialise sensor
      61        8248 : void AP_Proximity_LightWareSF45B::initialise()
      62             : {
      63             :     // check sensor is configured correctly
      64       24230 :     _init_complete = (_sensor_state.stream_data_type == PROXIMITY_SF45B_STREAM_DISTANCE_DATA_CM) &&
      65       15982 :                      (_sensor_state.update_rate == PROXIMITY_SF45B_DESIRED_UPDATE_RATE) &&
      66        7734 :                      (_sensor_state.streaming_fields == PROXIMITY_SF45B_DESIRED_FIELDS);
      67             : 
      68             :     // exit if initialisation requests have been sent within the last few seconds
      69        8248 :     uint32_t now_ms = AP_HAL::millis();
      70        8248 :     if ((now_ms - _last_init_ms) < PROXIMITY_SF45B_REINIT_INTERVAL_MS) {
      71        8240 :         return;
      72             :     }
      73           8 :     _last_init_ms = now_ms;
      74             : 
      75             :     // request stream rate and contents
      76           8 :     request_stream_start();
      77             : }
      78             : 
      79             : // request start of streaming of distances
      80           8 : void AP_Proximity_LightWareSF45B::request_stream_start()
      81             : {
      82             :     // request output rate
      83           8 :     send_message((uint8_t)MessageID::UPDATE_RATE, true, &PROXIMITY_SF45B_DESIRED_UPDATE_RATE, sizeof(PROXIMITY_SF45B_DESIRED_UPDATE_RATE));
      84             : 
      85             :     // request first return (unfiltered), and yaw angle
      86           8 :     send_message((uint8_t)MessageID::DISTANCE_OUTPUT, true, (const uint8_t*)&PROXIMITY_SF45B_DESIRED_FIELDS, sizeof(PROXIMITY_SF45B_DESIRED_FIELDS));
      87             : 
      88             :     // request start streaming of DISTANCE_DATA_CM messages
      89           8 :     send_message((uint8_t)MessageID::STREAM, true, (const uint8_t*)&PROXIMITY_SF45B_STREAM_DISTANCE_DATA_CM, sizeof(PROXIMITY_SF45B_STREAM_DISTANCE_DATA_CM));
      90           8 : }
      91             : 
      92             : // check for replies from sensor
      93        8248 : void AP_Proximity_LightWareSF45B::process_replies()
      94             : {
      95        8248 :     if (_uart == nullptr) {
      96           0 :         return;
      97             :     }
      98             : 
      99             :     // process up to 1K of characters per iteration
     100        8248 :     uint32_t nbytes = MIN(_uart->available(), 1024U);
     101       68124 :     while (nbytes-- > 0) {
     102             :         uint8_t c;
     103       59876 :         if (!_uart->read(c)) {
     104           0 :             continue;
     105             :         }
     106       59876 :         if (parse_byte(c)) {
     107        5990 :             process_message();
     108             :         }
     109             :     }
     110             : }
     111             : 
     112             : // process the latest message held in the _msg structure
     113        5990 : void AP_Proximity_LightWareSF45B::process_message()
     114             : {
     115             :     // process payload
     116        5990 :     switch ((MessageID)_msg.msgid) {
     117             : 
     118           8 :     case MessageID::DISTANCE_OUTPUT:
     119           8 :         if (_payload_recv == sizeof(uint32_t)) {
     120           8 :             _sensor_state.streaming_fields = UINT32_VALUE(_msg.payload[3], _msg.payload[2], _msg.payload[1], _msg.payload[0]);
     121             :         }
     122           8 :         break;
     123             : 
     124           8 :     case MessageID::STREAM:
     125           8 :         if (_payload_recv == sizeof(uint32_t)) {
     126           8 :             _sensor_state.stream_data_type = UINT32_VALUE(_msg.payload[3], _msg.payload[2], _msg.payload[1], _msg.payload[0]);
     127             :         }
     128           8 :         break;
     129             : 
     130           8 :     case MessageID::UPDATE_RATE:
     131           8 :         if (_payload_recv == 1) {
     132           8 :             _sensor_state.update_rate = _msg.payload[0];
     133             :         }
     134           8 :         break;
     135             : 
     136        5966 :     case MessageID::DISTANCE_DATA_CM: {
     137             :         // ignore distance messages until initialisation is complete
     138        5966 :         if (!_init_complete || (_payload_recv != (PROXIMITY_SF45B_DESIRED_FIELD_COUNT * 2))) {
     139             :             break;
     140             :         }
     141        5524 :         _last_distance_received_ms = AP_HAL::millis();
     142        5524 :         const float distance_m = _distance_filt.apply((int16_t)UINT16_VALUE(_msg.payload[1], _msg.payload[0])) * 0.01f;
     143        5524 :         const float angle_deg = correct_angle_for_orientation((int16_t)UINT16_VALUE(_msg.payload[3], _msg.payload[2]) * 0.01f);
     144             : 
     145             :         // if distance is from a new face then update distance, angle and boundary for previous face
     146             :         // get face from 3D boundary based on yaw angle to the object
     147        5524 :         const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
     148        5524 :         if (face != _face) {
     149         382 :             if (_face_distance_valid) {
     150         232 :                 frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance, state.instance);
     151             :             } else {
     152             :                 // mark previous face invalid
     153         150 :                 frontend.boundary.reset_face(_face, state.instance);
     154             :             }
     155             :             // record updated face
     156         382 :             _face = face;
     157         382 :             _face_yaw_deg = 0;
     158         382 :             _face_distance = INT16_MAX;
     159         382 :             _face_distance_valid = false;
     160             :         }
     161             : 
     162             :         // if distance is from a new minisector then update obstacle database using angle and distance from previous minisector
     163        5524 :         const uint8_t minisector = convert_angle_to_minisector(angle_deg);
     164        5524 :         if (minisector != _minisector) {
     165        3238 :             if ((_minisector != UINT8_MAX) && _minisector_distance_valid) {
     166        1849 :                 database_push(_minisector_angle, _minisector_distance);
     167             :             }
     168             :             // init mini sector
     169        3238 :             _minisector = minisector;
     170        3238 :             _minisector_angle = 0;
     171        3238 :             _minisector_distance = INT16_MAX;
     172        3238 :             _minisector_distance_valid = false;
     173             :         }
     174             : 
     175             :         // check reading is valid
     176        5524 :         if (!ignore_reading(angle_deg, distance_m) && (distance_m >= distance_min_m()) && (distance_m <= distance_max_m())) {
     177             :             // update shortest distance for this face
     178        3085 :             if (!_face_distance_valid || (distance_m < _face_distance)) {
     179         643 :                 _face_yaw_deg = angle_deg;
     180         643 :                 _face_distance = distance_m;
     181         643 :                 _face_distance_valid = true;
     182             :             }
     183             : 
     184             :             // update shortest distance for this mini sector
     185        3085 :             if (distance_m < _minisector_distance) {
     186        2097 :                 _minisector_angle = angle_deg;
     187        2097 :                 _minisector_distance = distance_m;
     188        2097 :                 _minisector_distance_valid = true;
     189             :             }
     190             :         }
     191        5524 :         break;
     192             :     }
     193             : 
     194           0 :     default:
     195             :         // ignore unsupported messages
     196           0 :         break;
     197             :     }
     198        5990 : }
     199             : 
     200             : // convert an angle (in degrees) to a mini sector number
     201        5524 : uint8_t AP_Proximity_LightWareSF45B::convert_angle_to_minisector(float angle_deg) const
     202             : {
     203        5524 :     return wrap_360(angle_deg + (PROXIMITY_SF45B_COMBINE_READINGS_DEG * 0.5f)) / PROXIMITY_SF45B_COMBINE_READINGS_DEG;
     204             : }
     205             : 
     206             : #endif // AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED