Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: move angle_max parameter to AP_Vehicle

Browse Source
This commit is contained in:
Randy Mackay 2013-10-18 17:03:31 +09:00 committed by Andrew Tridgell
parent 0521806cc6
commit 7b9a48107e
6 changed files with 30 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/ArduCopter.pde
View File

@ -144,6 +144,9 @@
#include "config.h"
#include "config_channels.h"
// key aircraft parameters passed to multiple libraries
static AP_Vehicle::MultiCopter aparm;
// Local modules
#include "Parameters.h"
#include "GCS.h"
@ -1145,7 +1148,7 @@ static void one_hz_loop()
wp_nav.set_althold_kP(g.pi_alt_hold.kP());
// update latest lean angle to navigation controller
wp_nav.set_lean_angle_max(g.angle_max);
wp_nav.set_lean_angle_max(aparm.angle_max);
// log battery info to the dataflash
if (g.log_bitmask & MASK_LOG_CURRENT) {

38
ArduCopter/Attitude.pde
View File

@ -43,16 +43,16 @@ static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int
}
// return filtered roll if no scaling required
if (g.angle_max == ROLL_PITCH_INPUT_MAX) {
if (aparm.angle_max == ROLL_PITCH_INPUT_MAX) {
roll_out = (int16_t)roll_in_filtered;
pitch_out = (int16_t)pitch_in_filtered;
return;
}
// check if angle_max has been updated and redo scaler
if (g.angle_max != _angle_max) {
_angle_max = g.angle_max;
_scaler = (float)g.angle_max/(float)ROLL_PITCH_INPUT_MAX;
if (aparm.angle_max != _angle_max) {
_angle_max = aparm.angle_max;
_scaler = (float)aparm.angle_max/(float)ROLL_PITCH_INPUT_MAX;
}
// convert pilot input to lean angle
@ -66,6 +66,9 @@ get_stabilize_roll(int32_t target_angle)
// angle error
target_angle = wrap_180_cd(target_angle - ahrs.roll_sensor);
// limit the error we're feeding to the PID
target_angle = constrain_int32(target_angle, -aparm.angle_max, aparm.angle_max);
// convert to desired rate
int32_t target_rate = g.pi_stabilize_roll.kP() * target_angle;
@ -84,6 +87,9 @@ get_stabilize_pitch(int32_t target_angle)
// angle error
target_angle = wrap_180_cd(target_angle - ahrs.pitch_sensor);
// limit the error we're feeding to the PID
target_angle = constrain_int32(target_angle, -aparm.angle_max, aparm.angle_max);
// convert to desired rate
int32_t target_rate = g.pi_stabilize_pitch.kP() * target_angle;
@ -139,10 +145,10 @@ get_acro_level_rates()
int32_t target_rate = 0;
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (roll_angle > g.angle_max){
target_rate = g.pi_stabilize_roll.get_p(g.angle_max-roll_angle);
}else if (roll_angle < -g.angle_max) {
target_rate = g.pi_stabilize_roll.get_p(-g.angle_max-roll_angle);
if (roll_angle > aparm.angle_max){
target_rate = g.pi_stabilize_roll.get_p(aparm.angle_max-roll_angle);
}else if (roll_angle < -aparm.angle_max) {
target_rate = g.pi_stabilize_roll.get_p(-aparm.angle_max-roll_angle);
}
}
roll_angle = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);
@ -156,10 +162,10 @@ get_acro_level_rates()
target_rate = 0;
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (pitch_angle > g.angle_max){
target_rate = g.pi_stabilize_pitch.get_p(g.angle_max-pitch_angle);
}else if (pitch_angle < -g.angle_max) {
target_rate = g.pi_stabilize_pitch.get_p(-g.angle_max-pitch_angle);
if (pitch_angle > aparm.angle_max){
target_rate = g.pi_stabilize_pitch.get_p(aparm.angle_max-pitch_angle);
}else if (pitch_angle < -aparm.angle_max) {
target_rate = g.pi_stabilize_pitch.get_p(-aparm.angle_max-pitch_angle);
}
}
pitch_angle = constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);
@ -321,8 +327,8 @@ get_roll_rate_stabilized_ef(int32_t stick_angle)
acro_roll = wrap_180_cd(acro_roll);
// ensure that we don't reach gimbal lock
if (labs(acro_roll) > g.angle_max) {
acro_roll = constrain_int32(acro_roll, -g.angle_max, g.angle_max);
if (labs(acro_roll) > aparm.angle_max) {
acro_roll = constrain_int32(acro_roll, -aparm.angle_max, aparm.angle_max);
angle_error = wrap_180_cd(acro_roll - ahrs.roll_sensor);
} else {
// angle error with maximum of +- max_angle_overshoot
@ -362,8 +368,8 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle)
acro_pitch = wrap_180_cd(acro_pitch);
// ensure that we don't reach gimbal lock
if (labs(acro_pitch) > g.angle_max) {
acro_pitch = constrain_int32(acro_pitch, -g.angle_max, g.angle_max);
if (labs(acro_pitch) > aparm.angle_max) {
acro_pitch = constrain_int32(acro_pitch, -aparm.angle_max, aparm.angle_max);
angle_error = wrap_180_cd(acro_pitch - ahrs.pitch_sensor);
} else {
// angle error with maximum of +- max_angle_overshoot

1
ArduCopter/Parameters.h
View File

@ -320,7 +320,6 @@ public:
AP_Int8 rssi_pin;
AP_Float rssi_range; // allows to set max voltage for rssi pin such as 5.0, 3.3 etc.
AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions
AP_Int16 angle_max; // maximum lean angle of the copter in centi-degrees
AP_Int32 angle_rate_max; // maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
AP_Int8 rc_feel_rp; // controls vehicle response to user input with 0 being extremely soft and 100 begin extremely crisp

3
ArduCopter/Parameters.pde
View File

@ -20,6 +20,7 @@
*/
#define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value : def} }
#define ASCALAR(v, name, def) { aparm.v.vtype, name, Parameters::k_param_ ## v, &aparm.v, {def_value : def} }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, {group_info : class::var_info} }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info : class::var_info} }
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &v, {group_info : class::var_info} }
@ -427,7 +428,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Description: Maximum lean angle in all flight modes
// @Range 1000 8000
// @User: Advanced
GSCALAR(angle_max, "ANGLE_MAX", DEFAULT_ANGLE_MAX),
ASCALAR(angle_max, "ANGLE_MAX", DEFAULT_ANGLE_MAX),
// @Param: ANGLE_RATE_MAX
// @DisplayName: Angle Rate max

2
ArduCopter/crash_check.pde
View File

@ -32,7 +32,7 @@ void crash_check()
}
// check angles
int32_t lean_max = g.angle_max + CRASH_CHECK_ANGLE_DEVIATION_CD;
int32_t lean_max = aparm.angle_max + CRASH_CHECK_ANGLE_DEVIATION_CD;
if (labs(ahrs.roll_sensor) > lean_max || labs(ahrs.pitch_sensor) > lean_max) {
inverted_count++;

2
ArduCopter/motors.pde
View File

@ -356,7 +356,7 @@ static void pre_arm_checks(bool display_failure)
}
// lean angle parameter check
if (g.angle_max < 1000 || g.angle_max > 8000) {
if (aparm.angle_max < 1000 || aparm.angle_max > 8000) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check ANGLE_MAX"));
}