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_VisualOdom_config.h"
      17             : 
      18             : #if AP_VISUALODOM_MAV_ENABLED
      19             : 
      20             : #include "AP_VisualOdom_MAV.h"
      21             : 
      22             : #include <AP_HAL/AP_HAL.h>
      23             : #include <AP_AHRS/AP_AHRS.h>
      24             : #include <AP_Logger/AP_Logger.h>
      25             : 
      26             : // consume vision pose estimate data and send to EKF. distances in meters
      27             : // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
      28        3365 : void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
      29             : {
      30        3365 :     const float scale_factor =  _frontend.get_pos_scale();
      31        3365 :     Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
      32             : 
      33        3365 :     if (_align_posxy || _align_posz) {
      34           0 :         if (align_position_to_ahrs(pos, _align_posxy, _align_posz)) {
      35           0 :             _align_posxy = _align_posz = false;
      36             :         }
      37             :     }
      38             : 
      39             :     // rotate position to align with vehicle
      40        3365 :     rotate_and_correct_position(pos);
      41             : 
      42        3365 :     posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
      43        3365 :     angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
      44             : 
      45             :     // record quality
      46        3365 :     _quality = quality;
      47             : 
      48             :     // send attitude and position to EKF if quality OK
      49        3365 :     bool consume = (_quality >= _frontend.get_quality_min());
      50        3365 :     if (consume) {
      51        3365 :         AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
      52             :     }
      53             : 
      54             :     // calculate euler orientation for logging
      55             :     float roll;
      56             :     float pitch;
      57             :     float yaw;
      58        3365 :     attitude.to_euler(roll, pitch, yaw);
      59             : 
      60             : #if HAL_LOGGING_ENABLED
      61             :     // log sensor data
      62        3365 :     Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, !consume, _quality);
      63             : #endif
      64             : 
      65             :     // record time for health monitoring
      66        3365 :     _last_update_ms = AP_HAL::millis();
      67        3365 : }
      68             : 
      69             : // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
      70             : // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
      71        1392 : void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter, int8_t quality)
      72             : {
      73             :     // record quality
      74        1392 :     _quality = quality;
      75             : 
      76             :     // send velocity to EKF if quality OK
      77        1392 :     bool consume = (_quality >= _frontend.get_quality_min());
      78        1392 :     if (consume) {
      79        1392 :         vel_err = fmaxf(vel_err, _frontend.get_vel_noise());
      80        1392 :         AP::ahrs().writeExtNavVelData(vel, vel_err, time_ms, _frontend.get_delay_ms());
      81             :     }
      82             : 
      83             :     // record time for health monitoring
      84        1392 :     _last_update_ms = AP_HAL::millis();
      85             : 
      86             : #if HAL_LOGGING_ENABLED
      87        1392 :     Write_VisualVelocity(remote_time_us, time_ms, vel, vel_err, reset_counter, !consume, _quality);
      88             : #endif
      89        1392 : }
      90             : 
      91             : #endif  // AP_VISUALODOM_MAV_ENABLED