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 "RPM_SITL.h"
      17             : 
      18             : #if AP_RPM_SIM_ENABLED
      19             : 
      20             : #include <AP_HAL/AP_HAL.h>
      21             : 
      22             : /*
      23             :    open the sensor in constructor
      24             : */
      25          93 : AP_RPM_SITL::AP_RPM_SITL(AP_RPM &_ap_rpm, uint8_t _instance, AP_RPM::RPM_State &_state) :
      26             :     AP_RPM_Backend(_ap_rpm, _instance, _state),
      27          93 :     sitl(AP::sitl())
      28             : {
      29          93 :     instance = _instance;
      30          93 : }
      31             : 
      32      460020 : void AP_RPM_SITL::update(void)
      33             : {
      34      460020 :     if (sitl == nullptr) {
      35           0 :         return;
      36             :     }
      37      460020 :     const uint32_t motor_mask = sitl->state.motor_mask;
      38      460020 :     uint8_t count = 0;
      39             :     // find the motor with the corresponding index
      40      593760 :     for (uint8_t i=0; i<32; i++) {
      41      593760 :         if (motor_mask & (1U<<i)) {
      42      460020 :             if (count == instance) {
      43      460020 :                 state.rate_rpm = sitl->state.rpm[i];
      44      460020 :                 break;
      45             :             }
      46           0 :             count++;
      47             :         }
      48             :     }
      49      460020 :     state.rate_rpm *= ap_rpm._params[state.instance].scaling;
      50      460020 :     state.signal_quality = 0.5f;
      51      460020 :     state.last_reading_ms = AP_HAL::millis();
      52             : 
      53             : }
      54             : 
      55             : #endif // AP_RPM_SIM_ENABLED