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             :   simple model of a servo. Model is:
      17             : 
      18             :     - time delay for transport protocol delay
      19             :     - slew limit
      20             :     - 2-pole butterworth
      21             : */
      22             : 
      23             : #include "ServoModel.h"
      24             : #include "SITL.h"
      25             : 
      26             : // SITL servo model parameters
      27             : const AP_Param::GroupInfo SITL::SIM::ServoParams::var_info[] = {
      28             :     // @Param: SPEED
      29             :     // @DisplayName: servo speed
      30             :     // @Description: servo speed (time for 60 degree deflection). If DELAY and FILTER are not set then this is converted to a 1p lowpass filter. If DELAY or FILTER are set then this is treated as a rate of change limit
      31             :     // @Units: s
      32             :     AP_GROUPINFO("SPEED",     1, ServoParams,  servo_speed, 0.14),
      33             : 
      34             :     // @Param: DELAY
      35             :     // @DisplayName: servo delay
      36             :     // @Description: servo delay
      37             :     // @Units: s
      38             :     AP_GROUPINFO("DELAY",     2, ServoParams,  servo_delay, 0.0),
      39             : 
      40             :     // @Param: FILTER
      41             :     // @DisplayName: servo filter
      42             :     // @Description: servo filter
      43             :     // @Units: Hz
      44             :     AP_GROUPINFO("FILTER",    3, ServoParams,  servo_filter, 0),
      45             :     
      46             :     AP_GROUPEND
      47             : };
      48             : 
      49             : /*
      50             :   simpler filter used when SIM_SERVO_FILTER and SIM_SERVO_DELAY are not set
      51             :   this filter is a 1p low pass based on SIM_SERVO_SPEED
      52             :  */
      53   333928295 : float ServoModel::apply_simple_filter(float v, float dt)
      54             : {
      55   333928295 :     const auto *sitl = AP::sitl();
      56   333928295 :     if (!is_positive(sitl->servo.servo_speed)) {
      57           0 :         return v;
      58             :     }
      59   333928295 :     const float cutoff = 1.0f / (2 * M_PI * sitl->servo.servo_speed);
      60   333928295 :     filter1p.set_cutoff_frequency(cutoff);
      61   333928295 :     return filter1p.apply(v, dt);
      62             : }
      63             : 
      64             : /*
      65             :   apply a filter for a servo model consistting of a delay, speed and 2p filter
      66             :  */
      67   344538527 : float ServoModel::apply_filter(float v, float dt)
      68             : {
      69   344538527 :     const auto *sitl = AP::sitl();
      70   344538527 :     if (!sitl) {
      71           6 :         return v;
      72             :     }
      73             : 
      74   678466816 :     if (!is_positive(sitl->servo.servo_delay) &&
      75   333928295 :         !is_positive(sitl->servo.servo_filter)) {
      76             :         // fallback to a simpler 1p filter model
      77   333928295 :         return apply_simple_filter(v, dt);
      78             :     }
      79             : 
      80             :     // apply delay
      81    10610226 :     if (sitl->servo.servo_delay > 0) {
      82    10610226 :         uint32_t delay_len = MAX(1,sitl->servo.servo_delay * sitl->loop_rate_hz);
      83    10610226 :         if (!delay) {
      84           6 :             delay = NEW_NOTHROW ObjectBuffer<float>();
      85             :         }
      86    10610226 :         if (delay->get_size() != delay_len) {
      87           6 :             delay->set_size(delay_len);
      88             :         }
      89    21220518 :         while (delay->space() > 0) {
      90    10610292 :             delay->push(v);
      91             :         }
      92    10610226 :         IGNORE_RETURN(delay->pop(v));
      93             :     }
      94             : 
      95             :     // apply slew limit
      96    10610226 :     if (sitl->servo.servo_speed > 0) {
      97             :         // assume SIM_SERVO_SPEED is time for 60 degrees
      98    10610226 :         float time_per_degree = sitl->servo.servo_speed / 60.0;
      99    10610226 :         float proportion_per_second = 1.0 / (angle_deg * time_per_degree);
     100    10610226 :         delta_max = proportion_per_second * dt;
     101    10610226 :         v = constrain_float(v, last_v-delta_max, last_v+delta_max);
     102    10610226 :         v = constrain_float(v, -1, 1);
     103    10610226 :         last_v = v;
     104             :     }
     105             : 
     106             :     // apply filter
     107    10610226 :     if (sitl->servo.servo_filter > 0) {
     108    10610226 :         filter.set_cutoff_frequency(sitl->loop_rate_hz, sitl->servo.servo_filter);
     109    10610226 :         v = filter.apply(v);
     110             :     }
     111             : 
     112    10610226 :     return v;
     113             : }
     114             : 
     115   135454372 : float ServoModel::filter_range(uint16_t pwm, float dt)
     116             : {
     117   135454372 :     const float v = (pwm - pwm_min)/float(pwm_max - pwm_min);
     118   135454372 :     return apply_filter(v, dt);
     119             : }
     120             : 
     121   209084155 : float ServoModel::filter_angle(uint16_t pwm, float dt)
     122             : {
     123   209084155 :     const float v = (pwm - 0.5*(pwm_max+pwm_min))/(0.5*float(pwm_max - pwm_min));
     124   209084155 :     return apply_filter(v, dt);
     125             : }
     126             : 
     127             : // return pwm required for current angle
     128     5295439 : float ServoModel::angle_pwm() const
     129             : {
     130     5295439 :     return angle() * (0.5*float(pwm_max - pwm_min)) + 0.5*(pwm_max+pwm_min);
     131             : }
     132             : 
     133     7073484 : void ServoModel::set_pwm_range(uint16_t _pwm_min, uint16_t _pwm_max)
     134             : {
     135     7073484 :     pwm_min = _pwm_min;
     136     7073484 :     pwm_max = _pwm_max;
     137     7073484 : }
     138             : 
     139     7073484 : void ServoModel::set_deflection(float _angle_deg)
     140             : {
     141     7073484 :     angle_deg = fabsf(_angle_deg);
     142     7073484 : }