Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: fixed yaw wrap in simulated AntennaTracker

Browse Source
This commit is contained in:
Andrew Tridgell 2015-06-01 14:08:45 +10:00
parent 2d2cc6c574
commit 96737a3982
2 changed files with 17 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/SITL/SIM_Tracker.cpp
View File

@ -31,28 +31,11 @@ Aircraft(home_str, frame_str)
*/
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 pitch_target = pitch_input*pitch_range;
float yaw_target = yaw_input*yaw_range;
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);
pitch_rate = constrain_float(pitch_target - pitch_current_relative, -pitchrate, pitchrate);
yaw_rate = constrain_float(yaw_target - yaw_current_relative, -yawrate, yawrate);
}
/*
@ -92,18 +75,12 @@ void Tracker::update(const struct sitl_input &input)
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;
pitch_current_relative = degrees(p) - zero_pitch;
yaw_current_relative = degrees(y) - zero_yaw;
float roll_current = degrees(r);
if (yaw_current_relative > 180) {
yaw_current_relative -= 360;
@ -124,6 +101,13 @@ void Tracker::update(const struct sitl_input &input)
pitch_rate = 0;
}
if (onoff) {
update_onoff_servos(yaw_rate, pitch_rate);
} else {
update_position_servos(delta_time, yaw_rate, pitch_rate);
}
// keep it level
float roll_rate = 0 - roll_current;

6
libraries/SITL/SIM_Tracker.h
View File

@ -43,13 +43,15 @@ private:
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
const float zero_yaw = 270; // yaw direction at startup
const float zero_pitch = 10; // pitch at startup
bool verbose = false;
uint64_t last_debug_us = 0;
float pitch_input;
float yaw_input;
float yaw_current_relative;
float pitch_current_relative;
void update_position_servos(float delta_time, float &yaw_rate, float &pitch_rate);
void update_onoff_servos(float &yaw_rate, float &pitch_rate);