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

Copter: remove jerk when entering RTL or AUTO

Browse Source 
Loiter target position was not being initialised properly.
Add a few more comments
This commit is contained in:
Randy Mackay 2013-05-03 16:58:00 +09:00
parent bd75de6a22
commit 647a93e78e
2 changed files with 32 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduCopter/ArduCopter.pde
View File

@ -3,8 +3,9 @@
#define THISFIRMWARE "ArduCopter V3.0.0-rc1"
/*
* ArduCopter Version 3.0
* Lead author: Jason Short
* Based on code and ideas from the Arducopter team: Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen, Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni
* Creator: Jason Short
* Lead Developer: Randy Mackay
* Based on code and ideas from the Arducopter team: Pat Hickey, Jose Julio, Jani Hirvinen, Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni
* Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier, Robert Lefebvre, Marco Robustini
*
* This firmware is free software; you can redistribute it and/or
@ -26,7 +27,7 @@
* HappyKillmore :Mavlink GCS
* Hein Hollander :Octo Support
* Igor van Airde :Control Law optimization
* Leonard Hall :Flight Dynamics, INAV throttle
* Leonard Hall :Flight Dynamics, Throttle, Loiter and Navigation Controllers
* Jonathan Challinger :Inertial Navigation
* Jean-Louis Naudin :Auto Landing
* Max Levine :Tri Support, Graphics
@ -35,7 +36,6 @@
* Jani Hiriven :Testing feedback
* John Arne Birkeland :PPM Encoder
* Jose Julio :Stabilization Control laws
* Randy Mackay :General development and release
* Marco Robustini :Lead tester
* Michael Oborne :Mission Planner GCS
* Mike Smith :Libraries, Coding support
@ -525,12 +525,12 @@ static uint8_t rtl_state;
// by the DCM through a few simple equations. They are used throughout the code where cos and sin
// would normally be used.
// The cos values are defaulted to 1 to get a decent initial value for a level state
static float cos_roll_x = 1;
static float cos_pitch_x = 1;
static float cos_yaw = 1;
static float sin_yaw = 1;
static float sin_roll = 1;
static float sin_pitch = 1;
static float cos_roll_x = 1.0;
static float cos_pitch_x = 1.0;
static float cos_yaw = 1.0;
static float sin_yaw;
static float sin_roll;
static float sin_pitch;
////////////////////////////////////////////////////////////////////////////////
// SIMPLE Mode

24
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -56,7 +56,22 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lo
_pid_pos_lon(pid_pos_lon),
_pid_rate_lat(pid_rate_lat),
_pid_rate_lon(pid_rate_lon),
_lean_angle_max(MAX_LEAN_ANGLE)
_loiter_last_update(0),
_wpnav_last_update(0),
_cos_yaw(1.0),
_sin_yaw(0.0),
_cos_pitch(1.0),
_desired_roll(0),
_desired_pitch(0),
_target(0,0,0),
_pilot_vel_forward_cms(0),
_pilot_vel_right_cms(0),
_target_vel(0,0,0),
_vel_last(0,0,0),
_lean_angle_max(MAX_LEAN_ANGLE),
dist_error(0,0),
desired_vel(0,0),
desired_accel(0,0)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -220,18 +235,23 @@ void AC_WPNav::set_destination(const Vector3f& destination)
/// set_origin_and_destination - set origin and destination using lat/lon coordinates
void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
{
// store origin and destination locations
_origin = origin;
_destination = destination;
Vector3f pos_delta = _destination - _origin;
bool climb = pos_delta.z >= 0; // climb vs descending lead to different leash lengths because speed_up_cms and speed_down_cms can be different
// calculate leash lengths
bool climb = pos_delta.z >= 0; // climbing vs descending leads to different leash lengths because speed_up_cms and speed_down_cms can be different
calculate_leash_length(climb); // update leash lengths and _vert_track_scale
// scale up z-axis position delta (i.e. distance) to make later leash length calculations simpler
pos_delta.z = pos_delta.z * _vert_track_scale;
_track_length = pos_delta.length();
_pos_delta_unit = pos_delta/_track_length;
// initialise intermediate point to the origin
_track_desired = 0;
_target = origin;
_reached_destination = false;
}