Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
B
Baitboat
Manage
Activity
Members
Labels
Plan
Issues
0
Issue boards
Milestones
Wiki
Code
Merge requests
0
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
OpenSource
Baitboat
Commits
0b011e3c
Commit
0b011e3c
authored
13 years ago
by
Jason Short
Browse files
Options
Downloads
Patches
Plain Diff
added option in code for Loiter specific gains
parent
101979ed
No related branches found
No related tags found
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
ArduCopter/navigation.pde
+16
-10
16 additions, 10 deletions
ArduCopter/navigation.pde
with
16 additions
and
10 deletions
ArduCopter/navigation.pde
+
16
−
10
View file @
0b011e3c
...
@@ -117,8 +117,12 @@ static void calc_loiter(int x_error, int y_error)
...
@@ -117,8 +117,12 @@ static void calc_loiter(int x_error, int y_error)
int16_t
y_iterm
=
g
.
pi_loiter_lat
.
get_i
(
y_error
,
dTnav
);
int16_t
y_iterm
=
g
.
pi_loiter_lat
.
get_i
(
y_error
,
dTnav
);
y_rate_error
=
y_target_speed
-
y_actual_speed
;
y_rate_error
=
y_target_speed
-
y_actual_speed
;
calc_nav_lon
(
x_rate_error
);
//nav_lon = g.pid_loiter_rate_lon.get_pid(x_rate_error, dTnav);
calc_nav_lat
(
y_rate_error
);
//nav_lat = g.pid_loiter_rate_lat.get_pid(y_rate_error, dTnav);
nav_lon
=
g
.
pid_nav_lon
.
get_pid
(
x_rate_error
,
dTnav
);
nav_lat
=
g
.
pid_nav_lat
.
get_pid
(
y_rate_error
,
dTnav
);
nav_lon
=
constrain
(
nav_lon
,
-
3000
,
3000
);
nav_lat
=
constrain
(
nav_lat
,
-
3000
,
3000
);
nav_lat
=
nav_lat
+
y_iterm
;
nav_lat
=
nav_lat
+
y_iterm
;
nav_lon
=
nav_lon
+
x_iterm
;
nav_lon
=
nav_lon
+
x_iterm
;
...
@@ -176,8 +180,11 @@ static void calc_nav_rate(int max_speed)
...
@@ -176,8 +180,11 @@ static void calc_nav_rate(int max_speed)
y_rate_error
=
constrain
(
y_rate_error
,
-
1000
,
1000
);
// added a rate error limit to keep pitching down to a minimum
y_rate_error
=
constrain
(
y_rate_error
,
-
1000
,
1000
);
// added a rate error limit to keep pitching down to a minimum
int16_t
y_iterm
=
g
.
pi_loiter_lat
.
get_i
(
y_rate_error
,
dTnav
);
int16_t
y_iterm
=
g
.
pi_loiter_lat
.
get_i
(
y_rate_error
,
dTnav
);
calc_nav_lon
(
x_rate_error
);
nav_lon
=
g
.
pid_nav_lon
.
get_pid
(
x_rate_error
,
dTnav
);
calc_nav_lat
(
y_rate_error
);
nav_lat
=
g
.
pid_nav_lat
.
get_pid
(
y_rate_error
,
dTnav
);
nav_lon
=
constrain
(
nav_lon
,
-
3000
,
3000
);
nav_lat
=
constrain
(
nav_lat
,
-
3000
,
3000
);
nav_lon
=
nav_lon
+
x_iterm
;
nav_lon
=
nav_lon
+
x_iterm
;
nav_lat
=
nav_lat
+
y_iterm
;
nav_lat
=
nav_lat
+
y_iterm
;
...
@@ -209,19 +216,18 @@ static void calc_nav_rate(int max_speed)
...
@@ -209,19 +216,18 @@ static void calc_nav_rate(int max_speed)
}
}
static
void
calc_nav_lon
(
int
rate
)
/*
static void calc_nav_lon(int rate)
{
{
nav_lon = g.pid_nav_lon.get_pid(rate, dTnav);
nav_lon = g.pid_nav_lon.get_pid(rate, dTnav);
//nav_lon = get_corrected_angle(rate, nav_lon);
nav_lon = constrain(nav_lon, -3000, 3000);
nav_lon = constrain(nav_lon, -3000, 3000);
}
}
static void calc_nav_lat(int rate)
static void calc_nav_lat(int rate)
{
{
nav_lat = g.pid_nav_lat.get_pid(rate, dTnav);
nav_lat = g.pid_nav_lat.get_pid(rate, dTnav);
//nav_lat = get_corrected_angle(rate, nav_lat);
nav_lat = constrain(nav_lat, -3000, 3000);
nav_lat = constrain(nav_lat, -3000, 3000);
}
}
*/
//static int16_t get_corrected_angle(int16_t desired_rate, int16_t rate_out)
//static int16_t get_corrected_angle(int16_t desired_rate, int16_t rate_out)
/*{
/*{
...
@@ -252,11 +258,11 @@ static void calc_loiter_pitch_roll()
...
@@ -252,11 +258,11 @@ static void calc_loiter_pitch_roll()
{
{
//Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y);
//Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y);
// rotate the vector
// rotate the vector
nav
_roll
=
(
float
)
nav_lon
*
sin_yaw_y
-
(
float
)
nav_lat
*
cos_yaw_x
;
auto
_roll
=
(
float
)
nav_lon
*
sin_yaw_y
-
(
float
)
nav_lat
*
cos_yaw_x
;
nav
_pitch
=
(
float
)
nav_lon
*
cos_yaw_x
+
(
float
)
nav_lat
*
sin_yaw_y
;
auto
_pitch
=
(
float
)
nav_lon
*
cos_yaw_x
+
(
float
)
nav_lat
*
sin_yaw_y
;
// flip pitch because forward is negative
// flip pitch because forward is negative
nav
_pitch
=
-
nav
_pitch
;
auto
_pitch
=
-
auto
_pitch
;
}
}
static
int16_t
calc_desired_speed
(
int16_t
max_speed
)
static
int16_t
calc_desired_speed
(
int16_t
max_speed
)
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment