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             :   antenna-tracker simulator class
      17             : */
      18             : 
      19             : #include "SIM_Tracker.h"
      20             : 
      21             : #include <stdio.h>
      22             : 
      23             : namespace SITL {
      24             : 
      25             : /*
      26             :   update function for position (normal) servos.
      27             : */
      28     2246756 : void Tracker::update_position_servos(float delta_time, float &yaw_rate, float &pitch_rate) const
      29             : {
      30     2246756 :     float pitch_target = pitch_input*pitch_range;
      31     2246756 :     float yaw_target = yaw_input*yaw_range;
      32             : 
      33     2246756 :     pitch_rate = constrain_float(pitch_target - pitch_current_relative, -pitchrate, pitchrate);
      34     2246756 :     yaw_rate   = constrain_float(yaw_target - yaw_current_relative, -yawrate, yawrate);
      35     2246756 : }
      36             : 
      37             : /*
      38             :   update function for onoff servos.
      39             :   These servos either move at a constant rate or are still
      40             :   Returns (yaw_rate,pitch_rate) tuple
      41             : */
      42           0 : void Tracker::update_onoff_servos(float &yaw_rate, float &pitch_rate) const
      43             : {
      44           0 :     if (fabsf(yaw_input) < 0.1) {
      45           0 :         yaw_rate = 0;
      46           0 :     } else if (yaw_input >= 0.1) {
      47           0 :         yaw_rate = yawrate;
      48             :     } else {
      49           0 :         yaw_rate = -yawrate;
      50             :     }
      51             : 
      52           0 :     if (fabsf(pitch_input) < 0.1) {
      53           0 :         pitch_rate = 0;
      54           0 :     } else if (pitch_input >= 0.1) {
      55           0 :         pitch_rate = pitchrate;
      56             :     } else {
      57           0 :         pitch_rate = -pitchrate;
      58             :     }
      59           0 : }
      60             : 
      61             : /*
      62             :   update state of tracker
      63             :  */
      64     2246756 : void Tracker::update(const struct sitl_input &input)
      65             : {
      66             :     // how much time has passed?
      67     2246756 :     float delta_time = frame_time_us * 1.0e-6f;
      68             : 
      69     2246756 :     float yaw_rate = 0.0f, pitch_rate = 0.0f;
      70             : 
      71     2246756 :     yaw_input = (input.servos[0]-1500)/500.0f;
      72     2246756 :     pitch_input = (input.servos[1]-1500)/500.0f;
      73             : 
      74             :     // implement yaw and pitch limits
      75             :     float r, p, y;
      76     2246756 :     dcm.to_euler(&r, &p, &y);
      77             : 
      78     2246756 :     pitch_current_relative = degrees(p) - zero_pitch;
      79     2246756 :     yaw_current_relative = degrees(y) - zero_yaw;
      80     2246756 :     float roll_current = degrees(r);
      81     2246756 :     if (yaw_current_relative > 180) {
      82           0 :         yaw_current_relative -= 360;
      83             :     }
      84     2246756 :     if (yaw_current_relative < -180) {
      85     1003134 :         yaw_current_relative += 360;
      86             :     }
      87     2246756 :     if (yaw_rate > 0 && yaw_current_relative >= yaw_range) {
      88           0 :         yaw_rate = 0;
      89             :     }
      90     2246756 :     if (yaw_rate < 0 && yaw_current_relative <= -yaw_range) {
      91           0 :         yaw_rate = 0;
      92             :     }
      93     2246756 :     if (pitch_rate > 0 && pitch_current_relative >= pitch_range) {
      94           0 :         pitch_rate = 0;
      95             :     }
      96     2246756 :     if (pitch_rate < 0 && pitch_current_relative <= -pitch_range) {
      97           0 :         pitch_rate = 0;
      98             :     }
      99             : 
     100     2246756 :     if (onoff) {
     101           0 :         update_onoff_servos(yaw_rate, pitch_rate);
     102             :     } else {
     103     2246756 :         update_position_servos(delta_time, yaw_rate, pitch_rate);
     104             :     }
     105             : 
     106             : 
     107             :     // keep it level
     108     2246756 :     float roll_rate = 0 - roll_current;
     109             : 
     110     2246756 :     if (time_now_us - last_debug_us > 2e6f && !onoff) {
     111         920 :         last_debug_us = time_now_us;
     112         920 :         printf("roll=%.1f pitch=%.1f yaw=%.1f rates=%.1f/%.1f/%.1f in=%.3f,%.3f\n",
     113             :                roll_current,
     114         920 :                pitch_current_relative,
     115         920 :                yaw_current_relative,
     116             :                roll_rate, pitch_rate, yaw_rate,
     117         920 :                yaw_input, pitch_input);
     118             :     }
     119             : 
     120     2246756 :     gyro = Vector3f(radians(roll_rate),radians(pitch_rate),radians(yaw_rate));
     121             : 
     122             :     // update attitude
     123     2246756 :     dcm.rotate(gyro * delta_time);
     124     2246756 :     dcm.normalize();
     125             : 
     126     2246756 :     Vector3f accel_earth = Vector3f(0, 0, -GRAVITY_MSS);
     127     2246756 :     accel_body = dcm.transposed() * accel_earth;
     128             : 
     129             :     // new velocity vector
     130     2246756 :     velocity_ef.zero();
     131     2246756 :     update_position();
     132     2246756 :     time_advance();
     133             : 
     134             :     // update magnetic field
     135     2246756 :     update_mag_field_bf();
     136             : 
     137     2246756 :     update_battery();
     138     2246756 : }
     139             : 
     140             : } // namespace SITL