mirror of https://github.com/ArduPilot/ardupilot
AC_AttitudeControl: Fix some typos
Fixed some typos found in the code.
This commit is contained in:
parent
3ba12d16db
commit
6c4d988631
|
@ -45,7 +45,7 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
|
||||||
|
|
||||||
// @Param: RATE_FF_ENAB
|
// @Param: RATE_FF_ENAB
|
||||||
// @DisplayName: Rate Feedforward Enable
|
// @DisplayName: Rate Feedforward Enable
|
||||||
// @Description: Controls whether body-frame rate feedfoward is enabled or disabled
|
// @Description: Controls whether body-frame rate feedforward is enabled or disabled
|
||||||
// @Values: 0:Disabled, 1:Enabled
|
// @Values: 0:Disabled, 1:Enabled
|
||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
AP_GROUPINFO("RATE_FF_ENAB", 5, AC_AttitudeControl, _rate_bf_ff_enabled, AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT),
|
AP_GROUPINFO("RATE_FF_ENAB", 5, AC_AttitudeControl, _rate_bf_ff_enabled, AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT),
|
||||||
|
@ -785,7 +785,7 @@ void AC_AttitudeControl::thrust_vector_rotation_angles(const Quaternion& attitud
|
||||||
// The direction of thrust is [0,0,-1] is any body-fixed frame, inc. body frame and target frame.
|
// The direction of thrust is [0,0,-1] is any body-fixed frame, inc. body frame and target frame.
|
||||||
const Vector3f thrust_vector_up{0.0f, 0.0f, -1.0f};
|
const Vector3f thrust_vector_up{0.0f, 0.0f, -1.0f};
|
||||||
|
|
||||||
// attitude_target and attitute_body are passive rotations from target / body frames to the NED frame
|
// attitude_target and attitude_body are passive rotations from target / body frames to the NED frame
|
||||||
|
|
||||||
// Rotating [0,0,-1] by attitude_target expresses (gets a view of) the target thrust vector in the inertial frame
|
// Rotating [0,0,-1] by attitude_target expresses (gets a view of) the target thrust vector in the inertial frame
|
||||||
Vector3f att_target_thrust_vec = attitude_target * thrust_vector_up; // target thrust vector
|
Vector3f att_target_thrust_vec = attitude_target * thrust_vector_up; // target thrust vector
|
||||||
|
|
|
@ -7,7 +7,7 @@
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
|
|
||||||
// 6DoF control is extracted from the existing copter code by treating desired angles as thrust angles rather than vehicle attitude.
|
// 6DoF control is extracted from the existing copter code by treating desired angles as thrust angles rather than vehicle attitude.
|
||||||
// Vehicle attitude is then set separately, typically the vehicle would matain 0 roll and pitch.
|
// Vehicle attitude is then set separately, typically the vehicle would maintain 0 roll and pitch.
|
||||||
// rate commands result in the vehicle behaving as a ordinary copter.
|
// rate commands result in the vehicle behaving as a ordinary copter.
|
||||||
|
|
||||||
// run lowest level body-frame rate controller and send outputs to the motors
|
// run lowest level body-frame rate controller and send outputs to the motors
|
||||||
|
|
|
@ -20,10 +20,10 @@ public:
|
||||||
|
|
||||||
// Command a Quaternion attitude with feedforward and smoothing
|
// Command a Quaternion attitude with feedforward and smoothing
|
||||||
// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity
|
// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity
|
||||||
// not used anywhere in current code, panic so this implementaiton is not overlooked
|
// not used anywhere in current code, panic so this implementation is not overlooked
|
||||||
void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) override;
|
void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) override;
|
||||||
/*
|
/*
|
||||||
override input functions to attitude controller and convert desired angles into thrust angles and substitute for osset angles
|
override input functions to attitude controller and convert desired angles into thrust angles and substitute for offset angles
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
|
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
#include "AC_CommandModel.h"
|
#include "AC_CommandModel.h"
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
|
||||||
// The Commmand Model class holds parameters that shape the pilot desired angular rate input. This class can
|
// The Command Model class holds parameters that shape the pilot desired angular rate input. This class can
|
||||||
// be expanded to hold the methods that shape the pilot desired input.
|
// be expanded to hold the methods that shape the pilot desired input.
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
|
@ -23,7 +23,7 @@ const AP_Param::GroupInfo AC_WeatherVane::var_info[] = {
|
||||||
|
|
||||||
// @Param: ENABLE
|
// @Param: ENABLE
|
||||||
// @DisplayName: Enable
|
// @DisplayName: Enable
|
||||||
// @Description: Enable weather vaning. When active, the aircraft will automatically yaw into wind when in a VTOL position controlled mode. Pilot yaw commands overide the weathervaning action.
|
// @Description: Enable weather vaning. When active, the aircraft will automatically yaw into wind when in a VTOL position controlled mode. Pilot yaw commands override the weathervaning action.
|
||||||
// @Values: -1:Only use during takeoffs or landing see weathervane takeoff and land override parameters,0:Disabled,1:Nose into wind,2:Nose or tail into wind,3:Side into wind,4:tail into wind
|
// @Values: -1:Only use during takeoffs or landing see weathervane takeoff and land override parameters,0:Disabled,1:Nose into wind,2:Nose or tail into wind,3:Side into wind,4:tail into wind
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
AP_GROUPINFO_FLAGS("ENABLE", 1, AC_WeatherVane, _direction, WVANE_PARAM_ENABLED, AP_PARAM_FLAG_ENABLE),
|
AP_GROUPINFO_FLAGS("ENABLE", 1, AC_WeatherVane, _direction, WVANE_PARAM_ENABLED, AP_PARAM_FLAG_ENABLE),
|
||||||
|
@ -139,7 +139,7 @@ bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, con
|
||||||
if (is_positive(_max_vel_xy) || is_positive(_max_vel_z)) {
|
if (is_positive(_max_vel_xy) || is_positive(_max_vel_z)) {
|
||||||
Vector3f vel_ned;
|
Vector3f vel_ned;
|
||||||
if (!AP::ahrs().get_velocity_NED(vel_ned) || // need speed estimate
|
if (!AP::ahrs().get_velocity_NED(vel_ned) || // need speed estimate
|
||||||
(is_positive(_max_vel_xy) && (vel_ned.xy().length_squared() > (_max_vel_xy*_max_vel_xy))) || // check xy speeed
|
(is_positive(_max_vel_xy) && (vel_ned.xy().length_squared() > (_max_vel_xy*_max_vel_xy))) || // check xy speed
|
||||||
(is_positive(_max_vel_z) && (fabsf(vel_ned.z) > _max_vel_z))) { // check z speed
|
(is_positive(_max_vel_z) && (fabsf(vel_ned.z) > _max_vel_z))) { // check z speed
|
||||||
reset();
|
reset();
|
||||||
return false;
|
return false;
|
||||||
|
|
|
@ -36,7 +36,7 @@ class AC_WeatherVane {
|
||||||
PITCH_ENABLE = (1<<0),
|
PITCH_ENABLE = (1<<0),
|
||||||
};
|
};
|
||||||
|
|
||||||
// Paramaters
|
// Parameters
|
||||||
AP_Int8 _direction;
|
AP_Int8 _direction;
|
||||||
AP_Float _gain;
|
AP_Float _gain;
|
||||||
AP_Float _min_dz_ang_deg;
|
AP_Float _min_dz_ang_deg;
|
||||||
|
|
Loading…
Reference in New Issue