SITL: added simulated antenna tracker

libraries/SITL/SIM_Tracker.cpp

@@ -0,0 +1,154 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  antenna-tracker simulator class
+*/
+
+#include "SIM_Tracker.h"
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include <stdio.h>
+
+Tracker::Tracker(const char *home_str, const char *frame_str) :
+Aircraft(home_str, frame_str)
+{}
+
+
+/*
+  update function for position (normal) servos.
+*/
+void Tracker::update_position_servos(float delta_time, float &yaw_rate, float &pitch_rate)
+{
+    float pitch_target = zero_pitch + pitch_input*pitch_range;
+    float yaw_target = zero_yaw + yaw_input*yaw_range;
+    while (yaw_target > 180) {
+        yaw_target -= 360;
+    }
+
+    float r, p, y;
+    dcm.to_euler(&r, &p, &y);
+
+    float pitch_current = degrees(p);
+    float yaw_current = degrees(y);
+
+    pitch_rate = constrain_float(pitch_target - pitch_current, -pitchrate, pitchrate);
+
+    float yaw_diff = yaw_target - yaw_current;
+    if (yaw_diff > 180) {
+        yaw_diff -= 360;
+    }
+    if (yaw_diff < -180) {
+        yaw_diff += 360;
+    }
+    yaw_rate = constrain_float(yaw_diff, -yawrate, yawrate);
+}
+
+/*
+  update function for onoff servos.
+  These servos either move at a constant rate or are still 
+  Returns (yaw_rate,pitch_rate) tuple
+*/
+void Tracker::update_onoff_servos(float &yaw_rate, float &pitch_rate)
+{
+    if (fabsf(yaw_input) < 0.1) {
+        yaw_rate = 0;
+    } else if (yaw_input >= 0.1) {
+        yaw_rate = yawrate;
+    } else {
+        yaw_rate = -yawrate;
+    }
+
+    if (fabsf(pitch_input) < 0.1) {
+        pitch_rate = 0;
+    } else if (pitch_input >= 0.1) {
+        pitch_rate = pitchrate;
+    } else {
+        pitch_rate = -pitchrate;
+    }
+}
+
+/*
+  update state of tracker
+ */
+void Tracker::update(const struct sitl_input &input)
+{
+    // how much time has passed?
+    float delta_time = frame_time_us * 1.0e-6f;
+    
+    float yaw_rate, pitch_rate;
+
+    yaw_input = (input.servos[0]-1500)/500.0f;
+    pitch_input = (input.servos[1]-1500)/500.0f;
+
+    if (onoff) {
+        update_onoff_servos(yaw_rate, pitch_rate);
+    } else {
+        update_position_servos(delta_time, yaw_rate, pitch_rate);
+    }
+
+    // implement yaw and pitch limits
+    float r, p, y;
+    dcm.to_euler(&r, &p, &y);
+
+    float pitch_current_relative = degrees(p) - zero_pitch;
+    float yaw_current_relative = degrees(y) - zero_yaw;
+    float roll_current = degrees(r);
+    if (yaw_current_relative > 180) {
+        yaw_current_relative -= 360;
+    }
+    if (yaw_current_relative < -180) {
+        yaw_current_relative += 360;
+    }
+    if (yaw_rate > 0 && yaw_current_relative >= yaw_range) {
+        yaw_rate = 0;
+    }
+    if (yaw_rate < 0 && yaw_current_relative <= -yaw_range) {
+        yaw_rate = 0;
+    }
+    if (pitch_rate > 0 && pitch_current_relative >= pitch_range) {
+        pitch_rate = 0;
+    }
+    if (pitch_rate < 0 && pitch_current_relative <= -pitch_range) {
+        pitch_rate = 0;
+    }
+
+    // keep it level
+    float roll_rate = 0 - roll_current;
+
+    if (time_now_us - last_debug_us > 2e6f && !onoff) {
+        last_debug_us = time_now_us;
+        printf("roll=%.1f pitch=%.1f yaw=%.1f rates=%.1f/%.1f/%.1f in=%.3f,%.3f\n",
+               roll_current,
+               pitch_current_relative, 
+               yaw_current_relative, 
+               roll_rate, pitch_rate, yaw_rate,
+               yaw_input, pitch_input);
+    }
+
+    gyro = Vector3f(radians(roll_rate),radians(pitch_rate),radians(yaw_rate));
+
+    // update attitude
+    dcm.rotate(gyro * delta_time);
+    dcm.normalize();
+
+    Vector3f accel_earth = Vector3f(0, 0, -GRAVITY_MSS);
+    accel_body = dcm.transposed() * accel_earth;
+
+    // new velocity vector
+    velocity_ef.zero();
+    update_position();
+}
+
+#endif

libraries/SITL/SIM_Tracker.h

@@ -0,0 +1,58 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  antenna-tracker simulator class
+*/
+
+#ifndef _SIM_TRACKER_H
+#define _SIM_TRACKER_H
+
+#include "SIM_Aircraft.h"
+
+/*
+  a antenna tracker simulator
+ */
+class Tracker : public Aircraft
+{
+public:
+    Tracker(const char *home_str, const char *frame_str);
+    void update(const struct sitl_input &input);
+
+    /* static object creator */
+    static Aircraft *create(const char *home_str, const char *frame_str) {
+        return new Tracker(home_str, frame_str);
+    }
+
+private:
+
+    const bool onoff = false;
+    const float yawrate = 9.0f;
+    const float pitchrate = 1.0f;
+    const float pitch_range = 45;
+    const float yaw_range = 170;
+    const float zero_yaw = 0;  // yaw direction at startup
+    const float zero_pitch = 0; // pitch at startup
+    bool verbose = false;
+    uint64_t last_debug_us = 0;
+
+    float pitch_input;
+    float yaw_input;
+
+    void update_position_servos(float delta_time, float &yaw_rate, float &pitch_rate);
+    void update_onoff_servos(float &yaw_rate, float &pitch_rate);
+};
+
+#endif