zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

自动绕8字优化，RTK绕8调整，返航先绕再回

zr-sdk-4.1.16
zbr 3 years ago
parent
250c85bed1
commit
5d8a84bcd5
9 changed files with 268 additions and 140 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. 18
      ArduCopter/mode.h
  2. 177
      ArduCopter/mode_auto.cpp
  3. 52
      ArduCopter/mode_rtl.cpp
  4. 2
      ArduCopter/version.h
  5. 10
      all.sh
  6. 122
      libraries/AC_WPNav/AC_Circle.cpp
  7. 12
      libraries/AC_WPNav/AC_Circle.h
  8. 2
      libraries/AC_WPNav/AC_WPNav.h
  9. 13
      libraries/AP_Mission/AP_Mission.cpp

18
ArduCopter/mode.h
Unescape View File

@ -410,6 +410,15 @@ private: @@ -410,6 +410,15 @@ private:
void payload_place_run_release();
AutoMode _mode = Auto_TakeOff; // controls which auto controller is run
AutoMode last_mode;
bool mode_change(AutoMode mode){
if(last_mode != mode){
last_mode = mode;
return true;
}else{
return false;
}
};
Location terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const;
@ -463,6 +472,8 @@ private: @@ -463,6 +472,8 @@ private:
// Loiter control
uint16_t loiter_time_max; // How long we should stay in Loiter Mode for mission scripting (time in seconds)
uint32_t loiter_time; // How long have we been loitering - The start time in millis
int32_t wp_yaw_cd;
bool circle_init_yaw;
struct {
bool reached_destination_xy : 1;
@ -493,6 +504,11 @@ private: @@ -493,6 +504,11 @@ private:
float descend_start_altitude;
float descend_max; // centimetres
} nav_payload_place;
uint16_t wp_nav_index; // nav 航点计数
bool first_wp_dec; // nav 航点计数
Location last_loc{};
};
#if AUTOTUNE_ENABLED == ENABLED
@ -1472,4 +1488,4 @@ private: @@ -1472,4 +1488,4 @@ private:
void warning_message(uint8_t message_n); //Handles output messages to the terminal
};
#endif
#endif

177
ArduCopter/mode_auto.cpp
Unescape View File

@ -45,6 +45,8 @@ bool ModeAuto::init(bool ignore_checks) @@ -45,6 +45,8 @@ bool ModeAuto::init(bool ignore_checks)
// start/resume the mission (based on MIS_RESTART parameter)
mission.start_or_resume();
wp_nav_index = 0; // nav 重新开始计数
first_wp_dec = false;
return true;
} else {
return false;
@ -57,21 +59,6 @@ bool ModeAuto::init(bool ignore_checks) @@ -57,21 +59,6 @@ bool ModeAuto::init(bool ignore_checks)
void ModeAuto::run()
{
// call the correct auto controller
// float proximity_alt_diff;
// AP_Proximity *proximity = AP::proximity();
// if (proximity && proximity->get_horizontal_distance(0,proximity_alt_diff)) {
// if(proximity_alt_diff < 30.0)
// {
// if(_mode != Auto_Loiter)
// gcs().send_text(MAV_SEVERITY_INFO, "Dist: %f, stop",proximity_alt_diff);
// _mode = Auto_Loiter;
// }
// }
switch (_mode) {
case Auto_TakeOff:
@ -158,6 +145,8 @@ void ModeAuto::takeoff_start(const Location& dest_loc) @@ -158,6 +145,8 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
Location dest(dest_loc);
// gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: dl:%d,%d,%d",dest_loc.lat,dest_loc.lng,dest_loc.alt);
// last_loc = dest_loc;
if (!copter.current_loc.initialised()) {
// vehicle doesn't know where it is ATM. We should not
// initialise our takeoff destination without knowing this!
@ -334,7 +323,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi @@ -334,7 +323,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi
void ModeAuto::circle_start()
{
_mode = Auto_Circle;
circle_init_yaw = false; // 绕圈转向下一航点初始化
// initialise circle controller
copter.circle_nav->init(copter.circle_nav->get_center());
}
@ -794,36 +783,36 @@ void ModeAuto::takeoff_run() @@ -794,36 +783,36 @@ void ModeAuto::takeoff_run()
}
}
}
if(g.zr_8_circle){ // 起飞到高度绕8字
if(g.zr_8_circle == 1){
const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
static uint8_t init_circle = false;
if(reached_wp_dest){
if(!init_circle){
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");
}else{
circle_run();
return;
}
}else{
init_circle = false;
if (copter.circle_nav->run_8_circle(reached_wp_dest))
{
circle_run();
return;
}
}
auto_takeoff_run();
}
// auto_wp_run - runs the auto waypoint controller
// called by auto_run at 100hz or more
void ModeAuto::wp_run()
{
// 到航点绕圈
if(g.zr_8_circle == 2 && !first_wp_dec){
const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
if(!reached_wp_dest){
circle_init_yaw = false;
}else{
if (copter.circle_nav->run_8_circle(reached_wp_dest)){
circle_run();
}
return;
}
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio) {
@ -933,17 +922,36 @@ void ModeAuto::rtl_run() @@ -933,17 +922,36 @@ void ModeAuto::rtl_run()
void ModeAuto::circle_run()
{
// call circle controller
copter.circle_nav->update();
// call z-axis position controller
pos_control->update_z_controller();
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
if(g.zr_8_circle > 0){ // 起飞到高度绕8字
int32_t initial_armed_bearing = wp_yaw_cd;
if(circle_init_yaw == false){ // 如果需要转向到下一个航点
if (abs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) { //转到角度
circle_init_yaw = true;
gcs().send_text(MAV_SEVERITY_INFO, "[%d]init ok:%d -- %d, %d",circle_init_yaw,initial_armed_bearing,ahrs.yaw_sensor,abs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)));
}else{ // 转向
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), initial_armed_bearing ,true);
return;
}
}else{ // 转向完成后，开始绕8字
copter.circle_nav->turn_2_circle();
}
pos_control->update_z_controller();
// Yaw 由 circle决定
attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), auto_yaw.yaw(), true);
}else{
copter.circle_nav->update();
// call z-axis position controller
pos_control->update_z_controller();
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), auto_yaw.yaw(), true);
}
}
}
@ -1206,7 +1214,7 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) @@ -1206,7 +1214,7 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
loiter_time_max = cmd.p1;
// Set wp navigation target
wp_start(target_loc);
// if no delay as well as not final waypoint set the waypoint as "fast"
AP_Mission::Mission_Command temp_cmd;
bool fast_waypoint = false;
@ -1223,6 +1231,34 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) @@ -1223,6 +1231,34 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
// if next command's lat, lon is specified then do not slowdown at this waypoint
if ((temp_cmd.content.location.lat != 0) || (temp_cmd.content.location.lng != 0)) {
fast_waypoint = true;
AP_Mission::Mission_Command next_2_cmd;
static int32_t last_yaw_cd;
if(mission.get_next_nav_cmd(cmd.index, next_2_cmd)){
float wp_distance = next_2_cmd.content.location.get_distance(temp_cmd.content.location);
if(wp_distance < 5.0){// 两个航点距离太近，正常是仿地降高度航点，则取再下一个航点
// gcs().send_text(MAV_SEVERITY_CRITICAL, "-wp too close:%.2f",wp_distance);
if(mission.get_next_nav_cmd(cmd.index+2, temp_cmd)){
wp_yaw_cd = next_2_cmd.content.location.get_bearing_to(temp_cmd.content.location);
// gcs().send_text(MAV_SEVERITY_CRITICAL, "-new rad:%d",wp_yaw_cd);
}
}else{
wp_yaw_cd = next_2_cmd.content.location.get_bearing_to(temp_cmd.content.location);
}
if(copter.wp_nav->get_fast_waypoint() == 2){
static float last_dist = 0;
int16_t delt_cd = abs(wp_yaw_cd - last_yaw_cd);
// delt_cd = (delt_cd > 0)? delt_cd:360+delt_cd;
fast_waypoint = (delt_cd > 6000)?false:true;
// gcs().send_text(MAV_SEVERITY_CRITICAL, "%d,a:%d,d:%d,f:%d,p:%.2f",cmd.index,wp_yaw_cd,delt_cd,fast_waypoint,wp_distance);
if(wp_distance - last_dist > 0 && first_wp_dec){ // P2.5转弯校准用 //TODO P2.5需要修改这里
// gcs().send_text(MAV_SEVERITY_CRITICAL, "---- First Turn, Do Sth ----");
}
last_dist = wp_distance;
last_yaw_cd = wp_yaw_cd;
}else{
fast_waypoint = copter.wp_nav->get_fast_waypoint();
}
}
}
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
@ -1237,7 +1273,7 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) @@ -1237,7 +1273,7 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
break;
}
fast_waypoint = copter.wp_nav->get_fast_waypoint();
// fast_waypoint = copter.wp_nav->get_fast_waypoint();
copter.wp_nav->set_fast_waypoint(fast_waypoint);
}
}
@ -1306,7 +1342,17 @@ void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd) @@ -1306,7 +1342,17 @@ void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
// calculate radius
uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
// 方向先转向下一个航点
AP_Mission::Mission_Command next_cmd;
AP_Mission::Mission_Command curr_cmd;
mission.get_next_nav_cmd(cmd.index+1, next_cmd);
mission.get_next_nav_cmd(cmd.index, curr_cmd);
// 计算下一个航点方向
wp_yaw_cd = curr_cmd.content.location.get_bearing_to(next_cmd.content.location);
if(g.zr_8_circle > 0){
circle_radius_m = 0;
}
// move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
circle_movetoedge_start(circle_center, circle_radius_m);
}
@ -1594,10 +1640,11 @@ bool ModeAuto::verify_takeoff() @@ -1594,10 +1640,11 @@ bool ModeAuto::verify_takeoff()
if (reached_wp_dest) {
copter.landinggear.set_position(AP_LandingGear::LandingGear_Retract);
}
if(g.zr_8_circle){ // 起飞结束判断，如果绕8字，则增加8字结束判断
if(g.zr_8_circle == 1){ //如果启用绕8字，并且是起飞到头顶绕8字，增加判断
return (reached_wp_dest && copter.circle_nav->get_circle_8_finish());
}else{
}else{ // 常规起飞判断，增加设置绕圈相关初始设置
copter.circle_nav->set_circle_8_finish(false);
first_wp_dec = false;
return reached_wp_dest;
}
}
@ -1904,8 +1951,25 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) @@ -1904,8 +1951,25 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
// play a tone
AP_Notify::events.waypoint_complete = 1;
}
gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
return true;
wp_nav_index += 1; // nav 计数，用于是否第一个航点绕8字判断
if(g.zr_8_circle == 2 && !first_wp_dec){
bool cyc_ret = copter.circle_nav->get_circle_8_finish();
if(cyc_ret){
// calculate stopping point
Vector3f stopping_point;
wp_nav->get_wp_stopping_point(stopping_point);
// initialise waypoint controller target to stopping point
wp_nav->set_wp_destination(stopping_point);
gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i,nav:%d",cmd.index,wp_nav_index);
first_wp_dec = true;
}
return cyc_ret;
}else{
gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i,nav:%d",cmd.index,wp_nav_index);
return true;
}
}
return false;
}
@ -1940,7 +2004,12 @@ bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd) @@ -1940,7 +2004,12 @@ bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
}
// check if we have completed circling
return fabsf(copter.circle_nav->get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
if(g.zr_8_circle > 0){ // 起飞到高度绕8字
return (copter.circle_nav->get_circle_8_finish());
}else{
return fabsf(copter.circle_nav->get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
}
}
// verify_spline_wp - check if we have reached the next way point using spline

52
ArduCopter/mode_rtl.cpp
Unescape View File

@ -44,47 +44,21 @@ void ModeRTL::run(bool disarm_on_land) @@ -44,47 +44,21 @@ void ModeRTL::run(bool disarm_on_land)
if (!motors->armed()) {
return;
}
if(g.zr_8_circle){
static bool init_circle = false;
static bool finish_circle = false;
if(_state != RTL_LoiterAtHome){
init_circle = false;
finish_circle = false;
}else{
if(!init_circle){
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);
// auto_yaw.set_mode(AUTO_YAW_LOOK_AHEAD);
if(g.zr_8_circle == 2){
static uint8_t flag = 0;
if(_state == RTL_ReturnHome){
// uint8_t flag = copter.circle_nav->run_rtl_8_circle(_state != RTL_LoiterAtHome);
copter.circle_nav->run_rtl_8_circle(&flag);
if(flag == 2){
auto_yaw.set_mode(AUTO_YAW_HOLD);
return;
}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
if(!ret){
attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
auto_yaw.set_mode(AUTO_YAW_RESETTOARMEDYAW);
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true);
}
// check if heading is within 2 degrees of heading when vehicle was armed
if (ret && abs(wrap_180_cd(ahrs.yaw_sensor-copter.initial_armed_bearing)) <= 200) {
finish_circle = true;
}
return;
}
attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true);
return; //等待
}else if(flag == 3){
auto_yaw.set_mode(AUTO_YAW_LOOK_AT_NEXT_WP);
gcs().send_text(MAV_SEVERITY_INFO, "RTL circle finish");
}
}else{
flag = 0;
}
}

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-rc6" //"ArduCopter V4.0.0"
#define THISFIRMWARE "ZRUAV v4.0.16-rc7" //"ArduCopter V4.0.0"
// the following line is parsed by the autotest scripts
#define FIRMWARE_VERSION 4,0,16,FIRMWARE_VERSION_TYPE_OFFICIAL

10
all.sh
Unescape View File

@ -1,20 +1,20 @@ @@ -1,20 +1,20 @@
./waf configure --board rs100
./waf clean
./waf copter
cp ./build/rs100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/致睿rs100-v4.0.16-RC5.px4
cp ./build/rs100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/致睿rs100-v4.0.16-RC6.px4
./waf configure --board rs100h
./waf clean
./waf copter
cp ./build/rs100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/致睿rs100h-v4.0.16-RC5.px4
cp ./build/rs100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/致睿rs100h-v4.0.16-RC6.px4
./waf configure --board d100
./waf clean
./waf copter
cp ./build/d100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/中海达d100-v4.0.16-RC5.px4
cp ./build/d100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/中海达d100-v4.0.16-RC6.px4
./waf configure --board d100h
./waf clean
./waf copter
cp ./build/d100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/中海达d100h-v4.0.16-RC5.px4
cp ./build/d100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/中海达d100h-v4.0.16-RC6.px4
./waf configure --board zr-hexa
./waf clean
./waf copter
cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/六轴m66-v4.0.16-RC5.px4
cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/六轴m66-v4.0.16-RC6.px4

122
libraries/AC_WPNav/AC_Circle.cpp
Unescape View File

@ -23,6 +23,7 @@ const AP_Param::GroupInfo AC_Circle::var_info[] = { @@ -23,6 +23,7 @@ const AP_Param::GroupInfo AC_Circle::var_info[] = {
// @User: Standard
AP_GROUPINFO("RATE", 1, AC_Circle, _rate, AC_CIRCLE_RATE_DEFAULT),
AP_GROUPINFO("ACCL", 2, AC_Circle, _accel, 10),
AP_GROUPEND
};
@ -39,14 +40,17 @@ AC_Circle::AC_Circle(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_Po @@ -39,14 +40,17 @@ AC_Circle::AC_Circle(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_Po
_angle_total(0.0f),
_angular_vel(0.0f),
_angular_vel_max(0.0f),
_angular_accel(0.0f)
_angular_accel(0.0f),
use_2_circle(true)
{
AP_Param::setup_object_defaults(this, var_info);
// init flags
_flags.panorama = false;
is_cw_turn = true;
}
/// init - initialise circle controller setting center specifically
/// caller should set the position controller's x,y and z speeds and accelerations before calling this
void AC_Circle::init(const Vector3f& center)
@ -116,12 +120,63 @@ void AC_Circle::set_rate(float deg_per_sec) @@ -116,12 +120,63 @@ void AC_Circle::set_rate(float deg_per_sec)
}
}
/**
* @brief 执行绕8字
*
* @return true
* @return false
*/
bool AC_Circle::run_8_circle(bool reached)
{
const bool reached_wp_dest = reached;
static uint8_t init_circle = false;
if(reached_wp_dest){
if(!init_circle){
init();
init_circle = true;
gcs().send_text(MAV_SEVERITY_INFO,"Run circle R=%.1f",(float)_radius);
}else{
return true;
}
}else{
init_circle = false;
}
return false;
}
uint8_t AC_Circle::run_rtl_8_circle(uint8_t *step)
{
uint8_t ret = 0;
// uint8_t _step = *step;
switch (*step)
{
case 0:
{ // 没到需要执行的转态，值设置
*step = 1;
}
break;
case 1:
{ //
init();
gcs().send_text(MAV_SEVERITY_INFO, "RTL circle R=%.1f", (float)_radius);
*step = 2;
}
break;
case 2:
{ //
if (!turn_2_circle())
{
_pos_control.update_z_controller();
}
else
{// 绕圈完成，退出
*step = 3;
}
}
break;
default:
break;
}
return ret;
}
bool AC_Circle::turn_2_circle()
{
// calculate dt
@ -144,35 +199,33 @@ bool AC_Circle::turn_2_circle() @@ -144,35 +199,33 @@ bool AC_Circle::turn_2_circle()
float angle_change = _angular_vel * dt;
float _fabs_total; // circle count,float
// calculate target position
Vector3f target;
static Vector3f target;
Vector3f new_pos;
// float bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x);
float bearing_rad = _ahrs.yaw;
static float last_bearing_rad ;
static float last_bearing_rad;
const Vector3f& stopping_point = _pos_control.get_pos_target();
switch (stage)
{
case 0:
// _angle = wrap_PI(_ahrs.yaw-M_PI);
_pos_control.set_desired_accel_xy(0.0f,0.0f);
_pos_control.set_desired_velocity_xy(0.0f,0.0f);
_pos_control.init_xy_controller();
// set initial position target to reasonable stopping point
_pos_control.set_target_to_stopping_point_xy();
_pos_control.set_target_to_stopping_point_z();
// get stopping point
// set circle center to circle_radius ahead of stopping point
_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;
gcs().send_text(MAV_SEVERITY_INFO, "--- AC_Circle x:%.2f,y:%.2f \n",_center.x,_center.y);
// calculate velocities
calc_velocities(true);
// set starting angle from vehicle heading
_angle_total = 0;
_angle = wrap_PI(_ahrs.yaw - M_PI/2);
last_bearing_rad = _ahrs.yaw;
// _angle = wrap_PI(_ahrs.yaw - M_PI/3);
_angle = wrap_PI(_ahrs.yaw-M_PI/2);
target.x = _center.x + _radius * cosf(_angle);
target.y = _center.y + _radius * sinf(_angle);
gcs().send_text(MAV_SEVERITY_INFO, "C2,%.2f,%.2f,%.2f,%.2f",
stopping_point.x,
stopping_point.y,
target.x,
target.y);
// 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);
last_bearing_rad = _ahrs.yaw;
// first_rad = _ahrs.yaw;
stage = 1;
break;
case 1:
@ -183,7 +236,7 @@ bool AC_Circle::turn_2_circle() @@ -183,7 +236,7 @@ bool AC_Circle::turn_2_circle()
_fabs_total = fabsf(get_angle_total()/M_2PI);
target.x = _center.x + _radius * cosf(_angle);
target.y = _center.y + _radius * sinf(_angle);
if(fabs(last_bearing_rad - bearing_rad) < 0.01 && _fabs_total > 0.9){
if((fabs(last_bearing_rad - bearing_rad) < 0.01 && _fabs_total > 0.5) || _fabs_total > 1.0){
_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 圆心正前改正左
// last_bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x);
@ -195,6 +248,11 @@ bool AC_Circle::turn_2_circle() @@ -195,6 +248,11 @@ bool AC_Circle::turn_2_circle()
target.y = _center.y - _radius * sinf(_angle);
_angle_total = 0;
stage = 2;
// gcs().send_text(MAV_SEVERITY_INFO, "rad1,%.2f,%.2f,%.2f,%.2f",
// last_bearing_rad,
// bearing_rad,
// fabs(last_bearing_rad - bearing_rad),
// _fabs_total);
}
}
break;
@ -206,15 +264,20 @@ bool AC_Circle::turn_2_circle() @@ -206,15 +264,20 @@ bool AC_Circle::turn_2_circle()
_fabs_total = fabsf(get_angle_total()/M_2PI);
target.x = _center.x + _radius * cosf(_angle);
target.y = _center.y - _radius * sinf(_angle);
if(fabs(last_bearing_rad - bearing_rad) < 0.01 && _fabs_total > 0.9){
if((fabs(last_bearing_rad - bearing_rad) < 0.01 && _fabs_total > 0.5) || _fabs_total > 0.95){
_angle_total = 0;
stage = 3;
// gcs().send_text(MAV_SEVERITY_INFO, "rad2,%.2f,%.2f,%.2f,%.2f",
// last_bearing_rad,
// bearing_rad,
// fabs(last_bearing_rad - bearing_rad),
// _fabs_total);
}
}
break;
case 3:
// _angle = wrap_PI(_ahrs.yaw-M_PI);
// gcs().send_text(MAV_SEVERITY_INFO, "[c]finish angle::%.2f,y:%.2f \n",_angle,_ahrs.yaw);
gcs().send_text(MAV_SEVERITY_INFO, "[c]finish angle::%.2f,y:%.2f,t:%.2f,%.2f \n",_angle,_ahrs.yaw,target.x,target.y);
stage = 4;
break;
case 4:
@ -225,7 +288,6 @@ bool AC_Circle::turn_2_circle() @@ -225,7 +288,6 @@ bool AC_Circle::turn_2_circle()
default:
break;
}
target.z = _pos_control.get_alt_target();
// update position controller target
_pos_control.set_xy_target(target.x, target.y);
@ -305,7 +367,7 @@ void AC_Circle::update() @@ -305,7 +367,7 @@ void AC_Circle::update()
void AC_Circle::get_closest_point_on_circle(Vector3f &result)
{
// return center if radius is zero
if (_radius <= 0) {
if (_radius <= 0 || use_2_circle) {
result = _center;
return;
}
@ -377,7 +439,7 @@ void AC_Circle::init_start_angle(bool use_heading) @@ -377,7 +439,7 @@ void AC_Circle::init_start_angle(bool use_heading)
// if use_heading is true
if (use_heading) {
_angle = wrap_PI(_ahrs.yaw-M_PI);
_angle = wrap_PI(_ahrs.yaw - M_PI/2);
} else {
// if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading)
const Vector3f &curr_pos = _inav.get_position();

12
libraries/AC_WPNav/AC_Circle.h
Unescape View File

@ -42,11 +42,12 @@ public: @@ -42,11 +42,12 @@ public:
/// get_angle_total - return total angle in radians that vehicle has circled
float get_angle_total() const { return _angle_total; }
bool get_circle_8_finish() const { return _circle_8_finish; }
void set_circle_8_finish(bool flg) { _circle_8_finish = flg; };
/// update - update circle controller
void update();
bool turn_2_circle(); // 一键8字用
bool get_circle_8_finish() const { return _circle_8_finish; } // 返回是否结束绕8字
bool turn_2_circle();
/// get desired roll, pitch which should be fed into stabilize controllers
float get_roll() const { return _pos_control.get_roll(); }
@ -66,8 +67,11 @@ public: @@ -66,8 +67,11 @@ public:
/// get bearing to target in centi-degrees
int32_t get_bearing_to_target() const { return _pos_control.get_bearing_to_target(); }
float get_accel() { return _accel; }
bool run_8_circle(bool reached);
uint8_t run_rtl_8_circle(uint8_t *step);
static const struct AP_Param::GroupInfo var_info[];
private:
// calc_velocities - calculate angular velocity max and acceleration based on radius and rate
@ -94,6 +98,7 @@ private: @@ -94,6 +98,7 @@ private:
// parameters
AP_Float _radius; // maximum horizontal speed in cm/s during missions
AP_Float _rate; // rotation speed in deg/sec
AP_Float _accel; // rotation speed in deg/sec
// internal variables
Vector3f _center; // center of circle in cm from home
@ -107,4 +112,5 @@ private: @@ -107,4 +112,5 @@ private:
bool is_cw_turn; // 绕8字方向
bool _circle_8_finish; // 绕8字完成
uint8_t stage; // 绕8字阶段
bool use_2_circle;
};

2
libraries/AC_WPNav/AC_WPNav.h
Unescape View File

@ -224,7 +224,7 @@ public: @@ -224,7 +224,7 @@ public:
static const struct AP_Param::GroupInfo var_info[];
bool get_fast_waypoint() const { return _fast_wp; }
int8_t get_fast_waypoint() const { return (int8_t)_fast_wp; }
protected:

13
libraries/AP_Mission/AP_Mission.cpp
Unescape View File

@ -2050,7 +2050,7 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i @@ -2050,7 +2050,7 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i
cmd_rtl_index -= 1;
// gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "resume %d",cmd_rtl_index);
if(cmd_rtl_index < 6 || num_commands() - cmd_rtl_index < 4)// 飛行的航點小於6則直接推出，否則讀取當前航點；飞完所有航点，也直接退出
if(cmd_rtl_index < 5 || num_commands() - cmd_rtl_index < 4)// 飛行的航點小於6則直接推出，否則讀取當前航點；飞完所有航点，也直接退出
{
need_change_cmd = 0;
// gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "[Resume] Need more waypoint,now:%d, need:6",cmd_rtl_index);
@ -2108,18 +2108,19 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i @@ -2108,18 +2108,19 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i
case 1:
if(cmd_temp.id == MAV_CMD_NAV_WAYPOINT)
{
read_cmd_from_storage(cmd_rtl_index,cmd_temp);
// cmd_temp.content.location.flags.relative_alt = 1;
rtlwp_alt = cmd_temp.content.location.alt;
// 判断第一个点是否有降高，两个航点一起
read_cmd_from_storage(cmd_rtl_index,cmd_temp);
new_cmd_i = cmd_i;
rtlwp_alt = cmd_temp.content.location.alt;
if(rtlwp_alt < takeoff_alt)
{
cmd_temp.content.location.alt = takeoff_alt;
cmd_rtl_index-=1;
}
replace_cmd(cmd_i, cmd_temp); // 替換 // 獲取第一個航點的序號
replace_cmd(cmd_i, cmd_temp); // 替換 // 獲取第一個航點的序號
cmd_step = 2;
new_cmd_i = cmd_i;
}
break;
case 2:

Write Preview
Loading…
Cancel
Save