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

绕8字增加上一个模式记录，退出绕圈时切换上一个模式，退出后加速度还原

mission-4.1.18
zbr 3 years ago
parent
cfdbbad37d
commit
66d5dbe5fe
9 changed files with 32 additions and 63 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. 2
      ArduCopter/Copter.h
  2. 9
      ArduCopter/Parameters.h
  3. 2
      ArduCopter/mode.cpp
  4. 1
      ArduCopter/mode.h
  5. 7
      ArduCopter/mode_auto.cpp
  6. 26
      ArduCopter/mode_circle.cpp
  7. 10
      ArduCopter/mode_rtl.cpp
  8. 2
      ArduCopter/version.h
  9. 36
      libraries/AC_WPNav/AC_Circle.cpp

2
ArduCopter/Copter.h
Unescape View File

@ -1033,6 +1033,7 @@ private: @@ -1033,6 +1033,7 @@ private:
Proxy_State proxy_state;
Proxy_Area proxy_area;
Mode::Number before_mode;
public:
void mavlink_delay_cb(); // GCS_Mavlink.cpp
void failsafe_check(); // failsafe.cpp
@ -1058,6 +1059,7 @@ public: @@ -1058,6 +1059,7 @@ public:
int8_t in_debug_mode;
uint8_t cacl_volt_pst;
uint8_t mkdir_step;
Mode::Number get_before_mode() const { return before_mode; };
};
extern Copter copter;

9
ArduCopter/Parameters.h
Unescape View File

@ -394,13 +394,9 @@ public: @@ -394,13 +394,9 @@ public:
k_param_zr_rtl_delay,
k_param_zr_use_rc,
k_param_hardware_flag,
<<<<<<< HEAD
k_param_zr_reg_date,
k_param_zr_8_circle,
k_param_zr_avd_wait,
=======
k_param_zr_8_circle,
>>>>>>> 098f02907d... 增加绕8字功能，一键8字，航线起飞和返航8字
// the k_param_* space is 9-bits in size
// 511: reserved
};
@ -515,14 +511,9 @@ public: @@ -515,14 +511,9 @@ public:
AP_Int16 zr_tk_delay;
AP_Int16 zr_rtl_delay;
AP_Int8 zr_use_rc;
<<<<<<< HEAD
AP_Int32 zr_reg_date;
AP_Int8 zr_8_circle;
AP_Int16 zr_avd_wait;
=======
AP_Int8 zr_8_circle;
>>>>>>> 098f02907d... 增加绕8字功能，一键8字，航线起飞和返航8字
// Note: keep initializers here in the same order as they are declared
// above.
Parameters()

2
ArduCopter/mode.cpp
Unescape View File

@ -190,6 +190,8 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) @@ -190,6 +190,8 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
if (mode == control_mode) {
control_mode_reason = reason;
return true;
}else{
before_mode = control_mode;
}
if(!zr_radio_valid() && \

1
ArduCopter/mode.h
Unescape View File

@ -597,6 +597,7 @@ private: @@ -597,6 +597,7 @@ private:
// Circle
bool pilot_yaw_override = false; // true if pilot is overriding yaw
float orig_yaw_accel;
};

7
ArduCopter/mode_auto.cpp
Unescape View File

@ -800,7 +800,12 @@ void ModeAuto::takeoff_run() @@ -800,7 +800,12 @@ void ModeAuto::takeoff_run()
static uint8_t init_circle = false;
if(reached_wp_dest){
if(!init_circle){
copter.circle_nav->set_radius(5 * 100.0f);
float radius = copter.circle_nav->get_radius();
if(radius < 500.0f){
copter.circle_nav->set_radius(5 * 100.0f);
}else if(radius > 2500.0f){
copter.circle_nav->set_radius(2000 * 100.0f);
}
copter.circle_nav->init();
init_circle = true;
gcs().send_text(MAV_SEVERITY_WARNING,"auto init_circle");

26
ArduCopter/mode_circle.cpp
Unescape View File

@ -16,7 +16,8 @@ bool ModeCircle::init(bool ignore_checks) @@ -16,7 +16,8 @@ bool ModeCircle::init(bool ignore_checks)
pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration());
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z);
orig_yaw_accel = attitude_control->get_accel_pitch_max();
// initialise circle controller including setting the circle center based on vehicle speed
copter.circle_nav->init();
@ -57,20 +58,19 @@ void ModeCircle::run() @@ -57,20 +58,19 @@ void ModeCircle::run()
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run circle controller
if(g.zr_8_circle){
bool ret = copter.circle_nav->turn_2_circle();
if(ret){
<<<<<<< HEAD
set_mode(Mode::Number::LOITER, ModeReason::REQUEST_CMD);
=======
set_mode(Mode::Number::GUIDED, ModeReason::REQUEST_CMD);
>>>>>>> 098f02907d... 增加绕8字功能，一键8字，航线起飞和返航8字
}
}else{
copter.circle_nav->update();
// copter.circle_nav->update();
bool ret = copter.circle_nav->turn_2_circle();
if(ret){
// pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy());
// 还原绕圈加速度和yaw加速度
pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration());
attitude_control->set_accel_yaw_max(orig_yaw_accel);
set_mode(copter.get_before_mode(), ModeReason::SCRIPTING);
}else{ // 限制绕圈加速度和yaw加速度
pos_control->set_max_accel_xy(50.0);
attitude_control->set_accel_yaw_max(9000);
}
// call attitude controller
if (pilot_yaw_override) {
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(copter.circle_nav->get_roll(),

10
ArduCopter/mode_rtl.cpp
Unescape View File

@ -50,10 +50,15 @@ void ModeRTL::run(bool disarm_on_land) @@ -50,10 +50,15 @@ void ModeRTL::run(bool disarm_on_land)
static bool finish_circle = false;
if(_state != RTL_LoiterAtHome){
init_circle = false;
finish_circle = false;
finish_circle = false;
}else{
if(!init_circle){
copter.circle_nav->set_radius(5 * 100.0f);
float radius = copter.circle_nav->get_radius();
if(radius < 500.0f){
copter.circle_nav->set_radius(5 * 100.0f);
}else if(radius > 2500.0f){
copter.circle_nav->set_radius(2000 * 100.0f);
}
copter.circle_nav->init();
init_circle = true;
gcs().send_text(MAV_SEVERITY_WARNING,"rtl init_circle,arm yaw:%ld",copter.initial_armed_bearing);
@ -63,7 +68,6 @@ void ModeRTL::run(bool disarm_on_land) @@ -63,7 +68,6 @@ void ModeRTL::run(bool disarm_on_land)
}else{
if(!finish_circle){
bool ret = copter.circle_nav->turn_2_circle();
// call z-axis position controller
pos_control->update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot

2
ArduCopter/version.h
Unescape View File

@ -6,7 +6,7 @@ @@ -6,7 +6,7 @@
#include "ap_version.h"
#define THISFIRMWARE "ZRUAV v4.0.16-AVDv2" //"ArduCopter V4.0.0"
#define THISFIRMWARE "ZRUAV v4.0.16-AVDv3" //"ArduCopter V4.0.0"
// the following line is parsed by the autotest scripts
#define FIRMWARE_VERSION 4,0,16,FIRMWARE_VERSION_TYPE_OFFICIAL

36
libraries/AC_WPNav/AC_Circle.cpp
Unescape View File

@ -104,10 +104,7 @@ void AC_Circle::init() @@ -104,10 +104,7 @@ void AC_Circle::init()
stage = 0;
is_cw_turn = true;
_circle_8_finish = false;
<<<<<<< HEAD
=======
// gcs().send_text(MAV_SEVERITY_INFO, "--- AC_Circle x:%.2f,y:%.2f \n",_center.x,_center.y);
>>>>>>> 098f02907d... 增加绕8字功能，一键8字，航线起飞和返航8字
}
/// set_circle_rate - set circle rate in degrees per second
@ -143,24 +140,14 @@ bool AC_Circle::turn_2_circle() @@ -143,24 +140,14 @@ bool AC_Circle::turn_2_circle()
_angular_vel -= fabsf(_angular_accel) * dt;
_angular_vel = MAX(_angular_vel, _angular_vel_max);
}
<<<<<<< HEAD
=======
>>>>>>> 098f02907d... 增加绕8字功能，一键8字，航线起飞和返航8字
//更新目标角和总的角度 update the target angle and total angle traveled
float angle_change = _angular_vel * dt;
float _fabs_total; // circle count,float
// calculate target position
Vector3f target;
<<<<<<< HEAD
const Vector3f curr_pos = _inav.get_position();
Vector3f new_pos;
float bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x);
=======
Vector3f new_pos;
// float bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x);
float bearing_rad = _ahrs.yaw;
>>>>>>> 098f02907d... 增加绕8字功能，一键8字，航线起飞和返航8字
static float last_bearing_rad ;
const Vector3f& stopping_point = _pos_control.get_pos_target();
switch (stage)
@ -175,16 +162,6 @@ bool AC_Circle::turn_2_circle() @@ -175,16 +162,6 @@ bool AC_Circle::turn_2_circle()
_pos_control.set_target_to_stopping_point_z();
// get stopping point
// set circle center to circle_radius ahead of stopping point
<<<<<<< HEAD
_center.x = stopping_point.x + _radius * _ahrs.cos_yaw();
_center.y = stopping_point.y + _radius * _ahrs.sin_yaw();
_center.z = stopping_point.z;
// calculate velocities
calc_velocities(true);
// set starting angle from vehicle heading
init_start_angle(true);
last_bearing_rad = bearing_rad;
=======
_center.x = stopping_point.x + _radius * cosf(_ahrs.yaw + M_PI_2);
_center.y = stopping_point.y + _radius * sinf(_ahrs.yaw + M_PI_2); // 圆心调整，+ M_PI_2 圆心正前改正右
_center.z = stopping_point.z;
@ -196,7 +173,6 @@ bool AC_Circle::turn_2_circle() @@ -196,7 +173,6 @@ bool AC_Circle::turn_2_circle()
_angle = wrap_PI(_ahrs.yaw - M_PI/2);
last_bearing_rad = _ahrs.yaw;
// gcs().send_text(MAV_SEVERITY_INFO, "[i]init ag::%.2f,y:%.2f,r:%.2f,d:%.2f\n",_angle,_ahrs.yaw,bearing_rad,bearing_rad * RAD_TO_DEG);
>>>>>>> 098f02907d... 增加绕8字功能，一键8字，航线起飞和返航8字
stage = 1;
break;
case 1:
@ -206,17 +182,6 @@ bool AC_Circle::turn_2_circle() @@ -206,17 +182,6 @@ bool AC_Circle::turn_2_circle()
_angle_total += angle_change;
_fabs_total = fabsf(get_angle_total()/M_2PI);
target.x = _center.x + _radius * cosf(_angle);
<<<<<<< HEAD
target.y = _center.y - _radius * sinf(_angle);
if(fabs(last_bearing_rad - bearing_rad) < 0.01 && _fabs_total > 0.9){
_center.x = _center.x + 2 * _radius* cosf(bearing_rad);
_center.y = _center.y + 2 * _radius* sinf(bearing_rad);
last_bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x);
is_cw_turn = false;
// calculate velocities
calc_velocities(false);
init_start_angle(false);
=======
target.y = _center.y + _radius * sinf(_angle);
if(fabs(last_bearing_rad - bearing_rad) < 0.01 && _fabs_total > 0.9){
_center.x = stopping_point.x + _radius * cosf(_ahrs.yaw - M_PI_2);
@ -228,7 +193,6 @@ bool AC_Circle::turn_2_circle() @@ -228,7 +193,6 @@ bool AC_Circle::turn_2_circle()
_angle = wrap_PI(-_ahrs.yaw - M_PI_2 ); // 逆时针绕与yaw反向，-pi/2提前1/4圈
target.x = _center.x + _radius * cosf(_angle);
target.y = _center.y - _radius * sinf(_angle);
>>>>>>> 098f02907d... 增加绕8字功能，一键8字，航线起飞和返航8字
_angle_total = 0;
stage = 2;
}

Write Preview
Loading…
Cancel
Save