Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
A
Ardupilot
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
Ardupilot
Commits
691a3d81
Commit
691a3d81
authored
10 years ago
by
Randy Mackay
Browse files
Options
Downloads
Patches
Plain Diff
AC_AttControlHeli: add passthrough_bf_roll_pitch_rate_yaw
parent
968f1d64
No related branches found
Branches containing commit
No related tags found
Tags containing commit
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
+63
-12
63 additions, 12 deletions
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
+9
-11
9 additions, 11 deletions
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
with
72 additions
and
23 deletions
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
+
63
−
12
View file @
691a3d81
...
...
@@ -61,6 +61,64 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = {
AP_GROUPEND
};
// passthrough_bf_roll_pitch_rate_yaw - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode
void
AC_AttitudeControl_Heli
::
passthrough_bf_roll_pitch_rate_yaw
(
float
roll_passthrough
,
float
pitch_passthrough
,
float
yaw_rate_bf
)
{
// store roll and pitch passthroughs
_passthrough_roll
=
roll_passthrough
;
_passthrough_pitch
=
pitch_passthrough
;
// set rate controller to use pass through
_flags_heli
.
flybar_passthrough
=
true
;
// set bf rate targets to current body frame rates (i.e. relax and be ready for vehicle to switch out of acro)
_rate_bf_desired
.
x
=
_ahrs
.
get_gyro
().
x
*
AC_ATTITUDE_CONTROL_DEGX100
;
_rate_bf_desired
.
y
=
_ahrs
.
get_gyro
().
y
*
AC_ATTITUDE_CONTROL_DEGX100
;
// accel limit desired yaw rate
if
(
_accel_y_max
>
0.0
f
)
{
float
rate_change_limit
=
_accel_y_max
*
_dt
;
float
rate_change
=
yaw_rate_bf
-
_rate_bf_desired
.
z
;
rate_change
=
constrain_float
(
rate_change
,
-
rate_change_limit
,
rate_change_limit
);
_rate_bf_desired
.
z
+=
rate_change
;
}
else
{
_rate_bf_desired
.
z
=
yaw_rate_bf
;
}
integrate_bf_rate_error_to_angle_errors
();
_angle_bf_error
.
x
=
0
;
_angle_bf_error
.
y
=
0
;
// update our earth-frame angle targets
Vector3f
angle_ef_error
;
frame_conversion_bf_to_ef
(
_angle_bf_error
,
angle_ef_error
);
_angle_ef_target
.
x
=
wrap_180_cd_float
(
angle_ef_error
.
x
+
_ahrs
.
roll_sensor
);
_angle_ef_target
.
y
=
wrap_180_cd_float
(
angle_ef_error
.
y
+
_ahrs
.
pitch_sensor
);
_angle_ef_target
.
z
=
wrap_360_cd_float
(
angle_ef_error
.
z
+
_ahrs
.
yaw_sensor
);
// handle flipping over pitch axis
if
(
_angle_ef_target
.
y
>
9000.0
f
)
{
_angle_ef_target
.
x
=
wrap_180_cd_float
(
_angle_ef_target
.
x
+
18000.0
f
);
_angle_ef_target
.
y
=
wrap_180_cd_float
(
18000.0
f
-
_angle_ef_target
.
x
);
_angle_ef_target
.
z
=
wrap_360_cd_float
(
_angle_ef_target
.
z
+
18000.0
f
);
}
if
(
_angle_ef_target
.
y
<
-
9000.0
f
)
{
_angle_ef_target
.
x
=
wrap_180_cd_float
(
_angle_ef_target
.
x
+
18000.0
f
);
_angle_ef_target
.
y
=
wrap_180_cd_float
(
-
18000.0
f
-
_angle_ef_target
.
x
);
_angle_ef_target
.
z
=
wrap_360_cd_float
(
_angle_ef_target
.
z
+
18000.0
f
);
}
// convert body-frame angle errors to body-frame rate targets
update_rate_bf_targets
();
// set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates
_rate_bf_target
.
x
=
_rate_bf_desired
.
x
;
_rate_bf_target
.
y
=
_rate_bf_desired
.
y
;
// add desired target to yaw
_rate_bf_target
.
z
+=
_rate_bf_desired
.
z
;
}
//
// rate controller (body-frame) methods
//
...
...
@@ -70,10 +128,11 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = {
void
AC_AttitudeControl_Heli
::
rate_controller_run
()
{
// call rate controllers and send output to motors object
// To-Do: should the outputs from get_rate_roll, pitch, yaw be int16_t which is the input to the motors library?
if
(
_flags_heli
.
flybar_passthrough
){
passthrough_to_motor_roll_pitch
();
}
else
{
// if using a flybar passthrough roll and pitch directly to motors
if
(
_flags_heli
.
flybar_passthrough
)
{
_motors
.
set_roll
(
_passthrough_roll
);
_motors
.
set_pitch
(
_passthrough_pitch
);
}
else
{
rate_bf_to_motor_roll_pitch
(
_rate_bf_target
.
x
,
_rate_bf_target
.
y
);
}
_motors
.
set_yaw
(
rate_bf_to_motor_yaw
(
_rate_bf_target
.
z
));
...
...
@@ -245,14 +304,6 @@ static LowPassFilterFloat rate_dynamics_filter; // Rate Dynamics filter
*/
}
// passthrough_to_motor_roll_pitch - passthrough the pilots roll and pitch inputs directly to swashplate for flybar acro mode
void
AC_AttitudeControl_Heli
::
passthrough_to_motor_roll_pitch
()
{
// output to motors
_motors
.
set_roll
(
_rc_roll
.
control_in
);
_motors
.
set_pitch
(
_rc_pitch
.
control_in
);
}
// rate_bf_to_motor_yaw - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second
float
AC_AttitudeControl_Heli
::
rate_bf_to_motor_yaw
(
float
rate_target_cds
)
{
...
...
This diff is collapsed.
Click to expand it.
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
+
9
−
11
View file @
691a3d81
...
...
@@ -22,18 +22,19 @@ public:
const
AP_Vehicle
::
MultiCopter
&
aparm
,
AP_MotorsHeli
&
motors
,
AC_P
&
p_angle_roll
,
AC_P
&
p_angle_pitch
,
AC_P
&
p_angle_yaw
,
AC_HELI_PID
&
pid_rate_roll
,
AC_HELI_PID
&
pid_rate_pitch
,
AC_HELI_PID
&
pid_rate_yaw
,
RC_Channel
&
rc_roll
,
RC_Channel
&
rc_pitch
AC_HELI_PID
&
pid_rate_roll
,
AC_HELI_PID
&
pid_rate_pitch
,
AC_HELI_PID
&
pid_rate_yaw
)
:
AC_AttitudeControl
(
ahrs
,
aparm
,
motors
,
p_angle_roll
,
p_angle_pitch
,
p_angle_yaw
,
pid_rate_roll
,
pid_rate_pitch
,
pid_rate_yaw
),
_rc_roll
(
rc_roll
),
_rc_pitch
(
rc_pitch
)
_passthrough_roll
(
0
),
_passthrough_pitch
(
0
)
{
AP_Param
::
setup_object_defaults
(
this
,
var_info
);
}
// passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw
void
passthrough_bf_roll_pitch_rate_yaw
(
float
roll_passthrough
,
float
pitch_passthrough
,
float
yaw_rate_bf
);
// rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors
// should be called at 100hz or more
virtual
void
rate_controller_run
();
...
...
@@ -57,12 +58,8 @@ private:
uint8_t
limit_pitch
:
1
;
// 1 if we have requested larger pitch angle than swash can physically move
uint8_t
limit_yaw
:
1
;
// 1 if we have requested larger yaw angle than tail servo can physically move
uint8_t
leaky_i
:
1
;
// 1 if we should use leaky i term for body-frame rate to motor stage
uint8_t
flybar_passthrough
:
1
;
// 1 if we should pass through pilots input directly to swash-plate
uint8_t
flybar_passthrough
:
1
;
// 1 if we should pass through pilots
roll & pitch
input directly to swash-plate
}
_flags_heli
;
// references to external libraries
RC_Channel
&
_rc_roll
;
RC_Channel
&
_rc_pitch
;
//
// body-frame rate controller
...
...
@@ -79,8 +76,6 @@ private:
// get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle
virtual
int16_t
get_angle_boost
(
int16_t
throttle_pwm
);
// roll and pitch inputs are sent directly to motor outputs (servos) direct flybar control.
void
passthrough_to_motor_roll_pitch
();
// LPF filters to act on Rate Feedforward terms to linearize output.
// Due to complicated aerodynamic effects, feedforwards acting too fast can lead
...
...
@@ -89,6 +84,9 @@ private:
LowPassFilterInt32
roll_feedforward_filter
;
LowPassFilterInt32
yaw_feedforward_filter
;
// pass through for roll and pitch
int16_t
_passthrough_roll
;
int16_t
_passthrough_pitch
;
};
#endif //AC_ATTITUDECONTROL_HELI_H
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