zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

降落速度限制,函数调整,仿真通过

mission-4.1.18
孤帆远影Faraway 3 years ago committed by zbr
parent
9a35132e95
commit
7cd90ad398
7 changed files with 80 additions and 79 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 31
      ArduCopter/Attitude.cpp
  2. 1
      ArduCopter/Copter.h
  3. 11
      ArduCopter/Parameters.cpp
  4. 2
      ArduCopter/Parameters.h
  5. 97
      ArduCopter/mode.cpp
  6. 3
      ArduCopter/mode.h
  7. 14
      ArduCopter/mode_loiter.cpp

31
ArduCopter/Attitude.cpp
Unescape View File

@ -156,33 +156,42 @@ void Copter::rotate_body_frame_to_NE(float &x, float &y) @@ -156,33 +156,42 @@ void Copter::rotate_body_frame_to_NE(float &x, float &y)
// It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
uint16_t Copter::get_pilot_speed_dn()
{
int16_t speed_dn_now ;
if (g2.pilot_speed_dn == 0) {
return abs(g.pilot_speed_up);
} else {
return abs(g2.pilot_speed_dn);
}
}
uint16_t Copter::get_speed_dn_slow()
{
int16_t speed_dn_now = 0 ;
if(get_alt_above_ground_cm() < g2.pilot_slow_dn2_alt)
speed_dn_now = g2.pilot_speed_dn2;
else if(get_alt_above_ground_cm() < g2.land_alt_low)
if(get_alt_above_ground_cm() < g2.land_alt_low){ // 2m
speed_dn_now = g.land_speed;
}
else if(get_alt_above_ground_cm() < g.land_slow_2nd_alt){ // 15m
speed_dn_now = g.land_speed_2nd;
}
else if(get_alt_above_ground_cm() < g.land_slow_3nd_alt){ // 30m
speed_dn_now = g.land_speed_3nd;
}
else
{
speed_dn_now = g2.pilot_speed_dn;
}
if (speed_dn_now == 0) {
return abs(g.pilot_speed_up);
} else {
return abs(speed_dn_now);
}
// if (g2.pilot_speed_dn == 0) {
// return abs(g.pilot_speed_up);
// } else {
// return abs(g2.pilot_speed_dn);
// }
}
int32_t Copter::get_alt_above_ground_cm()
{
int32_t alt_above_ground;
if (rangefinder_alt_ok() && current_loc.alt < g.land_slow_2nd_alt) {
if (rangefinder_alt_ok() && current_loc.alt < g.land_slow_3nd_alt) {
alt_above_ground = rangefinder_state.alt_cm_filt.get();
} else {
bool navigating = pos_control->is_active_xy();

1
ArduCopter/Copter.h
Unescape View File

@ -685,6 +685,7 @@ private: @@ -685,6 +685,7 @@ private:
void set_accel_throttle_I_from_pilot_throttle();
void rotate_body_frame_to_NE(float &x, float &y);
uint16_t get_pilot_speed_dn();
uint16_t get_speed_dn_slow();
int32_t get_alt_above_ground_cm();
UpDownState updownStatus = UpDown_TakeOffStart;
#if ADSB_ENABLED == ENABLED

11
ArduCopter/Parameters.cpp
Unescape View File

@ -258,9 +258,9 @@ const AP_Param::Info Copter::var_info[] = { @@ -258,9 +258,9 @@ const AP_Param::Info Copter::var_info[] = {
// @Range: 30 200
// @Increment: 10
// @User: Standard
GSCALAR(land_speed, "LAND_SPEED", 100),
GSCALAR(land_speed_2nd, "LAND_SPEED_2ND", 50),
GSCALAR(land_speed_3nd, "LAND_SPEED_3ND", 100),
GSCALAR(land_speed, "LAND_SPEED", 50),
GSCALAR(land_speed_2nd, "LAND_SPEED_2ND", 100),
GSCALAR(land_speed_3nd, "LAND_SPEED_3ND", 200),
GSCALAR(land_slow_2nd_alt, "LAND_SL_2ND_ALT", 1500),
GSCALAR(land_slow_3nd_alt, "LAND_SL_3ND_ALT", 3000),
GSCALAR(land_lock_alt, "LAND_LOCK_ALT", 100),
@ -1022,11 +1022,6 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { @@ -1022,11 +1022,6 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_SUBGROUPINFO(arot, "AROT_", 37, ParametersG2, AC_Autorotation),
#endif
AP_GROUPINFO("PILOT_SPEED_DN2", 38, ParametersG2, pilot_speed_dn2, 100),
AP_GROUPINFO("PILOT_DN2_ALT", 39, ParametersG2, pilot_slow_dn2_alt, 100),
AP_GROUPEND
};

2
ArduCopter/Parameters.h
Unescape View File

@ -618,8 +618,6 @@ public: @@ -618,8 +618,6 @@ public:
// Additional pilot velocity items
AP_Int16 pilot_speed_dn;
AP_Int16 pilot_speed_dn2;
AP_Int16 pilot_slow_dn2_alt;
// Land alt final stage
AP_Int16 land_alt_low;

97
ArduCopter/mode.cpp
Unescape View File

@ -545,12 +545,7 @@ void Mode::land_run_vertical_control(bool pause_descent) @@ -545,12 +545,7 @@ void Mode::land_run_vertical_control(bool pause_descent)
const float precland_min_descent_speed = 10.0f;
// modify by @Brown , Staging land speed control
int16_t land_speed_now = g.land_speed;
if(get_alt_above_ground_cm() < g.land_slow_2nd_alt)
land_speed_now = g.land_speed_2nd;
else if(get_alt_above_ground_cm() < 3000)
land_speed_now = g.land_speed_3nd;
int16_t land_speed_now = get_speed_dn_slow();
float cmb_rate = 0;
if (!pause_descent) {
@ -560,14 +555,12 @@ void Mode::land_run_vertical_control(bool pause_descent) @@ -560,14 +555,12 @@ void Mode::land_run_vertical_control(bool pause_descent)
} else {
max_land_descent_velocity = pos_control->get_max_speed_down();
}
// Don't speed up for landing.
// max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed));
max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(land_speed_now));
max_land_descent_velocity = MAX(max_land_descent_velocity, -abs(land_speed_now));
// Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low.
cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt);
// Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed.
// cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed));
cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(land_speed_now));
@ -648,41 +641,8 @@ void Mode::land_run_horizontal_control() @@ -648,41 +641,8 @@ void Mode::land_run_horizontal_control()
pos_control->override_vehicle_velocity_xy(-target_vel_rel);
}
#endif
static uint8_t prt_once;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 2) && \
g.land_lock_alt && \
(copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO);
if(!prt_once && land_lock_attitude)
gcs().send_text(MAV_SEVERITY_INFO, "land lock sitl mode");
#else
bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 2) && \
copter.rangefinder_alt_ok() && g.land_lock_alt && \
(copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO);
#endif
if(land_lock_attitude)
{
if(prt_once == 0)
{
gcs().send_text(MAV_SEVERITY_INFO, "Land ready to enter attitude lock mode");
copter.Log_Write_Event(DATA_LAND_READY_LOCK);
prt_once = 1;
}
if(copter.rangefinder_state.alt_cm_filt.get() < g.land_lock_alt){
target_roll = 0;
target_pitch = 0;
target_yaw_rate = 0;
loiter_nav->soften_for_landing();
if(prt_once == 1)
{
gcs().send_text(MAV_SEVERITY_INFO, "Land Lock attitude!");
copter.Log_Write_Event(DATA_LAND_LOCK_ATT);
prt_once = 2;
}
}
}
land_lock_att_control(&target_roll,&target_pitch,&target_yaw_rate);
// process roll, pitch inputs
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
@ -722,6 +682,50 @@ void Mode::land_run_horizontal_control() @@ -722,6 +682,50 @@ void Mode::land_run_horizontal_control()
}
}
void Mode::land_lock_att_control(float *p_roll,float *p_pitch,float *p_yaw_rate)
{
float *target_roll = p_roll;
float *target_pitch = p_pitch;
float *target_yaw_rate = p_yaw_rate;
static uint8_t prt_once;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 2) && \
g.land_lock_alt && \
(copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO);
if(!prt_once && land_lock_attitude)
gcs().send_text(MAV_SEVERITY_INFO, "land lock sitl mode");
#else
bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 2) && \
copter.rangefinder_alt_ok() && g.land_lock_alt && \
(copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO);
#endif
if(land_lock_attitude)
{
if(prt_once == 0)
{
gcs().send_text(MAV_SEVERITY_INFO, "Land ready to enter attitude lock mode");
copter.Log_Write_Event(DATA_LAND_READY_LOCK);
prt_once = 1;
}
if(copter.rangefinder_state.alt_cm_filt.get() < g.land_lock_alt){
*target_roll = 0;
*target_pitch = 0;
*target_yaw_rate = 0;
loiter_nav->soften_for_landing();
if(prt_once == 1)
{
gcs().send_text(MAV_SEVERITY_INFO, "Land Lock attitude!");
copter.Log_Write_Event(DATA_LAND_LOCK_ATT);
prt_once = 2;
}
}
}else{
prt_once = 0;
}
}
float Mode::throttle_hover() const
{
return motors->get_throttle_hover();
@ -884,3 +888,8 @@ uint16_t Mode::get_pilot_speed_dn() @@ -884,3 +888,8 @@ uint16_t Mode::get_pilot_speed_dn()
{
return copter.get_pilot_speed_dn();
}
uint16_t Mode::get_speed_dn_slow()
{
return copter.get_speed_dn_slow();
}

3
ArduCopter/mode.h
Unescape View File

@ -101,6 +101,7 @@ protected: @@ -101,6 +101,7 @@ protected:
// functions to control landing
// in modes that support landing
void land_run_horizontal_control();
void land_lock_att_control(float *p_roll,float *p_pitch,float *p_yaw_rate);
void land_run_vertical_control(bool pause_descent = false);
// return expected input throttle setting to hover:
@ -245,7 +246,7 @@ public: @@ -245,7 +246,7 @@ public:
void set_throttle_takeoff(void);
float get_avoidance_adjusted_climbrate(float target_rate);
uint16_t get_pilot_speed_dn(void);
uint16_t get_pilot_speed_dn2(void);
uint16_t get_speed_dn_slow(void);
// end pass-through functions
};

14
ArduCopter/mode_loiter.cpp
Unescape View File

@ -80,19 +80,7 @@ void ModeLoiter::run() @@ -80,19 +80,7 @@ void ModeLoiter::run()
float takeoff_climb_rate = 0.0f;
// @Brown, loiter land auto slow down
int16_t land_speed_now = get_pilot_speed_dn();
// static int16_t last_land_speed = land_speed_now;
// if(get_alt_above_ground_cm() < g2.pilot_slow_dn2_alt)
// land_speed_now = g2.pilot_speed_dn2;
// else if(get_alt_above_ground_cm() < g2.land_alt_low)
// land_speed_now = g.land_speed;
// if(last_land_speed != land_speed_now)
// {
// // gcs().send_text(MAV_SEVERITY_INFO, "vz:%4d,dn2:%4d,dn:%4d",land_speed_now,g2.pilot_speed_dn2,g.land_speed);
// gcs().send_text(MAV_SEVERITY_INFO, "vz:%4d",land_speed_now);
// last_land_speed = land_speed_now;
// }
int16_t land_speed_now = get_speed_dn_slow();
// initialize vertical speed and acceleration
// pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);

Write Preview
Loading…
Cancel
Save