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             :    This program is distributed in the hope that it will be useful,
       7             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       8             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
       9             :    GNU General Public License for more details.
      10             :    You should have received a copy of the GNU General Public License
      11             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      12             :  */
      13             : 
      14             : #include "AP_Proximity_Scripting.h"
      15             : 
      16             : #if HAL_PROXIMITY_ENABLED && AP_SCRIPTING_ENABLED
      17             : 
      18             : #include <AP_HAL/AP_HAL.h>
      19             : #include <ctype.h>
      20             : #include <stdio.h>
      21             : 
      22             : extern const AP_HAL::HAL& hal;
      23             : 
      24             : #define PROXIMITY_SCRIPTING_TIMEOUT_MS 1500 // distance messages must arrive within this many milliseconds
      25             : 
      26             : // update the state of the sensor
      27           0 : void AP_Proximity_Scripting::update(void)
      28             : {
      29             :     // check for timeout and set health status
      30           0 :     if ((_last_update_ms == 0 || (AP_HAL::millis() - _last_update_ms > PROXIMITY_SCRIPTING_TIMEOUT_MS)) &&
      31           0 :         (_last_upward_update_ms == 0 || (AP_HAL::millis() - _last_upward_update_ms > PROXIMITY_SCRIPTING_TIMEOUT_MS))) {
      32           0 :         set_status(AP_Proximity::Status::NoData);
      33             :     } else {
      34           0 :         set_status(AP_Proximity::Status::Good);
      35             :     }
      36           0 : }
      37             : 
      38             : // Set max and min range of the sensor. Only needs to be set once
      39           0 : bool AP_Proximity_Scripting::set_distance_min_max(float min, float max)
      40             : {
      41           0 :     if (min >= max) {
      42           0 :         return false;
      43             :     }
      44           0 :     _distance_min = min;
      45           0 :     _distance_max = max;
      46           0 :     return true;
      47             : }
      48             : 
      49             : // get distance upwards in meters. returns true on success
      50           0 : bool AP_Proximity_Scripting::get_upward_distance(float &distance) const
      51             : {
      52           0 :     if ((_last_upward_update_ms != 0) && (AP_HAL::millis() - _last_upward_update_ms <= PROXIMITY_SCRIPTING_TIMEOUT_MS)) {
      53           0 :         distance = _distance_upward;
      54           0 :         return true;
      55             :     }
      56           0 :     return false;
      57             : }
      58             : 
      59             : // handle script distance messages
      60           0 : bool AP_Proximity_Scripting::handle_script_distance_msg(float dist_m, float yaw_deg, float pitch_deg, bool push_to_boundary)
      61             : {
      62           0 :     _last_update_ms = AP_HAL::millis();
      63             : 
      64           0 :     Vector3f current_pos;
      65           0 :     Matrix3f body_to_ned;
      66           0 :     const bool database_ready = database_prepare_for_push(current_pos, body_to_ned);
      67             : 
      68           0 :     if (dist_m < distance_min_m() || dist_m > distance_max_m() || is_zero(dist_m)) {
      69             :         // message isn't healthy
      70           0 :         return false;
      71             :     }
      72             : 
      73             :     // store upward distance
      74           0 :     if (is_equal(pitch_deg, 90.f)) {
      75           0 :         _distance_upward = dist_m;
      76           0 :         _last_upward_update_ms = _last_update_ms;
      77           0 :         return true;
      78             :     }
      79             : 
      80           0 :     yaw_deg = correct_angle_for_orientation(yaw_deg);
      81             : 
      82           0 :     if (ignore_reading(pitch_deg, yaw_deg, dist_m, false)) {
      83             :         // obstacle is probably near ground or out of range
      84           0 :         return false;
      85             :     }
      86             : 
      87             :     // allot to correct layer and sector based on calculated pitch and yaw
      88           0 :     const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(pitch_deg, yaw_deg);
      89             : 
      90             :     // add to temp boundary
      91           0 :     temp_boundary.add_distance(face, pitch_deg, yaw_deg, dist_m);
      92             : 
      93           0 :     if (push_to_boundary) {
      94           0 :         temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
      95           0 :         temp_boundary.reset();
      96             :     }
      97             : 
      98           0 :     if (database_ready) {
      99           0 :         database_push(yaw_deg, pitch_deg, dist_m, _last_update_ms, current_pos, body_to_ned);
     100             :     }
     101             : 
     102           0 :     return true;
     103             : }
     104             : 
     105             : // handle script vector messages
     106           0 : bool AP_Proximity_Scripting::handle_script_3d_msg(const Vector3f &vec_to_obstacle, bool push_to_boundary)
     107             : {
     108             :     // convert to FRU
     109           0 :     const Vector3f obstacle(vec_to_obstacle.x, vec_to_obstacle.y, vec_to_obstacle.z * -1.0f);
     110             : 
     111             :     // extract yaw and pitch from Obstacle Vector
     112           0 :     const float yaw = wrap_360(degrees(atan2f(obstacle.y, obstacle.x)));
     113           0 :     const float pitch = wrap_180(degrees(M_PI_2 - atan2f(obstacle.xy().length(), obstacle.z)));
     114             : 
     115             :     // now simply handle as a distance msg
     116           0 :     return handle_script_distance_msg(obstacle.length(), yaw, pitch, push_to_boundary);
     117             : }
     118             : 
     119             : // update the temporary (buffer) boundary
     120           0 : bool AP_Proximity_Scripting::update_virtual_boundary()
     121             : {
     122           0 :     temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
     123           0 :     temp_boundary.reset();
     124           0 :     return true;
     125             : }
     126             : 
     127             : #endif // HAL_PROXIMITY_ENABLED && AP_SCRIPTING_ENABLED