From edb54cabb59370d81aca1d585598bc6a326e11c3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 1 Jun 2015 12:53:30 +1000 Subject: [PATCH] SITL: added simulated antenna tracker --- libraries/SITL/SIM_Tracker.cpp | 154 +++++++++++++++++++++++++++++++++ libraries/SITL/SIM_Tracker.h | 58 +++++++++++++ 2 files changed, 212 insertions(+) create mode 100644 libraries/SITL/SIM_Tracker.cpp create mode 100644 libraries/SITL/SIM_Tracker.h diff --git a/libraries/SITL/SIM_Tracker.cpp b/libraries/SITL/SIM_Tracker.cpp new file mode 100644 index 0000000000..d27862eba0 --- /dev/null +++ b/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 . + */ +/* + antenna-tracker simulator class +*/ + +#include "SIM_Tracker.h" +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include + +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 diff --git a/libraries/SITL/SIM_Tracker.h b/libraries/SITL/SIM_Tracker.h new file mode 100644 index 0000000000..01db4366fd --- /dev/null +++ b/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 . + */ +/* + 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