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

第一个点不绕圈问题

celiu-4.0.17-rc8
zbr 3 years ago
parent
0d5128651b
commit
39022f59ea
4 changed files with 98 additions and 83 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. 168
      ArduCopter/mode_auto.cpp
  2. 9
      ArduCopter/mode_rtl.cpp
  3. 2
      ArduCopter/version.h
  4. 2
      d100h.sh

168
ArduCopter/mode_auto.cpp
Unescape View File

@ -47,7 +47,6 @@ bool ModeAuto::init(bool ignore_checks) @@ -47,7 +47,6 @@ bool ModeAuto::init(bool ignore_checks)
mission.start_or_resume();
wp_nav_index = 0; // nav 重新开始计数
updown_state = 0; // 起飞悬停状态清0
// gcs().send_text(MAV_SEVERITY_INFO, "auto init:up:%d,wp:%d",updown_state,wp_nav_index);
return true;
} else {
return false;
@ -150,8 +149,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc) @@ -150,8 +149,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
if (g.zr_tk_delay > 0 && g.zr_tk_req_alt > 0 && updown_state == 0){ // 有设置起飞到高度悬停
updown_state = 1;
dest.alt = g.zr_tk_req_alt;
gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
copter.Log_Write_Data(DATA_NOT_BOTTOMED, (uint16_t)copter.updownStatus);
copter.Log_Write_Data(DATA_UPDOWN_STATUS, (uint16_t)copter.updownStatus);
}
if (!copter.current_loc.initialised()) {
// vehicle doesn't know where it is ATM. We should not
@ -233,7 +231,6 @@ void ModeAuto::wp_start(const Location& dest_loc) @@ -233,7 +231,6 @@ void ModeAuto::wp_start(const Location& dest_loc)
if (copter.updownStatus < UpDown_InMission)
{
copter.updownStatus = UpDown_InMission;
gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
copter.Log_Write_Data(DATA_UPDOWN_STATUS, (uint16_t)copter.updownStatus);
gcs().send_message(MSG_ZR_FLYING_STATUS);
}
@ -445,7 +442,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) @@ -445,7 +442,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
break;
case MAV_CMD_NAV_LOITER_TURNS: //18 Loiter N Times
// do_roi(cmd);
// do_roi(cmd); // 绕圈指向中间
do_circle(cmd);
break;
@ -770,7 +767,6 @@ void ModeAuto::takeoff_run() @@ -770,7 +767,6 @@ void ModeAuto::takeoff_run()
{
updown_state = 3; // 到达悬停高度，状态更新到等待
copter.updownStatus = UpDown_RequestClimb;
gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
copter.Log_Write_Data(DATA_UPDOWN_STATUS, (uint16_t)copter.updownStatus);
gcs().send_message(MSG_ZR_FLYING_STATUS);
countdown = g.zr_tk_delay;
@ -785,7 +781,6 @@ void ModeAuto::takeoff_run() @@ -785,7 +781,6 @@ void ModeAuto::takeoff_run()
else if (countdown <= 0){
gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: 执行继续任务");
copter.updownStatus = UpDown_ContinueClimb;
gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
copter.Log_Write_Data(DATA_UPDOWN_STATUS, (uint16_t)copter.updownStatus);
gcs().send_message(MSG_ZR_FLYING_STATUS);
updown_state = 4;
@ -806,7 +801,6 @@ void ModeAuto::takeoff_run() @@ -806,7 +801,6 @@ void ModeAuto::takeoff_run()
}else if(copter.updownStatus > UpDown_RequestClimb){
if(updown_state < 4){ // 地面站弹框确定，发送新的值，状态位同步更新
updown_state = 4;
gcs().send_text(MAV_SEVERITY_INFO, "gcs ctl updownStatus:%d",copter.updownStatus);
takeoff_start(last_loc);
}
}
@ -819,6 +813,55 @@ void ModeAuto::takeoff_run() @@ -819,6 +813,55 @@ void ModeAuto::takeoff_run()
// called by auto_run at 100hz or more
void ModeAuto::wp_run()
{
// 到航点绕圈
if(g.zr_8_circle == 2 && wp_nav_index == 0){
const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
static uint32_t last_100ms;
uint32_t tnow = AP_HAL::millis();
if (tnow - last_100ms > 100) {
Vector3f stopping_point;
last_100ms = tnow;
wp_nav->get_wp_stopping_point(stopping_point);
AP::logger().Write("ATES", "TimeUS,Indx,Rech,IniY,Ymod,px,py,pz", "QHBBBfff",
AP_HAL::micros64(),
wp_nav_index,
reached_wp_dest,
circle_init_yaw,
auto_yaw.mode(),
stopping_point.x,
stopping_point.y,
stopping_point.z );
}
if(reached_wp_dest){
static autopilot_yaw_mode last_yaw_mode;
if(circle_init_yaw == false){ // 如果需要转向到下一个航点
if (abs(wrap_180_cd(ahrs.yaw_sensor-wp_yaw_cd)) <= 200) { //转到角度
circle_init_yaw = true;
auto_yaw.set_mode(last_yaw_mode);
copter.circle_nav->init();
gcs().send_text(MAV_SEVERITY_INFO,"Auto run circle");
}else{
// wp_nav->set_wp_destination(last_loc);// 设置第一个WayPoint点为期望值
if (auto_yaw.mode() != AUTO_YAW_FIXED) { // 需要转角度，yaw mode使用AUTO_YAW_FIXED
// wp_nav->set_wp_destination(last_loc);// 设置第一个WayPoint点为期望值
last_yaw_mode = auto_yaw.mode();
auto_yaw.set_mode(AUTO_YAW_FIXED);
}
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), wp_yaw_cd,true);
}
return;
}
}
if(circle_init_yaw)
{ // 如果需要转向到下一个航点
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);
return;
}
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio) {
@ -845,39 +888,6 @@ void ModeAuto::wp_run() @@ -845,39 +888,6 @@ void ModeAuto::wp_run()
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control->update_z_controller();
// 到航点绕圈
if(g.zr_8_circle == 2 && wp_nav_index == 0){
const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
if(reached_wp_dest){
static autopilot_yaw_mode last_yaw_mode;
if(circle_init_yaw == false){ // 如果需要转向到下一个航点
if (abs(wrap_180_cd(ahrs.yaw_sensor-wp_yaw_cd)) <= 200) { //转到角度
circle_init_yaw = true;
auto_yaw.set_mode(last_yaw_mode);
copter.circle_nav->init();
gcs().send_text(MAV_SEVERITY_INFO,"Auto run circle");
// gcs().send_text(MAV_SEVERITY_INFO, "circ init: y: %d -- %d ",wp_yaw_cd,ahrs.yaw_sensor);
}else{
if (auto_yaw.mode() != AUTO_YAW_FIXED) { // 需要转角度，yaw mode使用AUTO_YAW_FIXED
last_yaw_mode = auto_yaw.mode();
// gcs().send_text(MAV_SEVERITY_INFO, "auto_yaw: y: %d ",auto_yaw.mode());
auto_yaw.set_mode(AUTO_YAW_FIXED);
}
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), wp_yaw_cd,true);
}
return;
}else{ // 如果需要转向到下一个航点
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);
return;
}
}
}
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
@ -969,19 +979,14 @@ void ModeAuto::circle_run() @@ -969,19 +979,14 @@ void ModeAuto::circle_run()
if (abs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) { //转到角度
circle_init_yaw = true;
auto_yaw.set_mode(last_yaw_mode);
// gcs().send_text(MAV_SEVERITY_INFO, "circ init: y: %d -- %d ",initial_armed_bearing,ahrs.yaw_sensor);
}else{
if(wp_nav->set_wp_destination(last_loc)) { // 设置第一个WayPoint点为期望值
// if(wp_nav->set_wp_destination(last_loc)) { // 设置第一个WayPoint点为期望值
if (auto_yaw.mode() != AUTO_YAW_FIXED) { // 需要转角度，yaw mode使用AUTO_YAW_FIXED
last_yaw_mode = auto_yaw.mode();
// gcs().send_text(MAV_SEVERITY_INFO, "auto_yaw: y: %d ",auto_yaw.mode());
auto_yaw.set_mode(AUTO_YAW_FIXED);
}
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), initial_armed_bearing,true);
}else{ // 第一个WayPoint点获取失败，直接绕圈
circle_init_yaw = true;
gcs().send_text(MAV_SEVERITY_INFO, "--- circle turn false ---");
}
// }
return;
}
@ -1269,8 +1274,24 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) @@ -1269,8 +1274,24 @@ 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(wp_nav_index < 2){ // 传给绕圈用，只有第一个wp需要绕圈，后面就不赋值了
if(wp_nav_index < 1){ // 传给绕圈用，只有第一个wp需要绕圈，后面就不赋值了
last_loc = target_loc;
AP_Mission::Mission_Command next_cmd;
for(int i = cmd.index; i < cmd.index+5;i++)
{
mission.read_cmd_from_storage(i,next_cmd); // 读取原本航点
// gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "read[%d]: %d",i,cmd_rtl.id);
if (mission.is_nav_cmd(next_cmd)) { // 是否是Waypoint
float wp_distance = cmd.content.location.get_distance(next_cmd.content.location);
if(wp_distance < 5.0 && wp_nav_index < 2){ // 第一个wp需要转向相关。两个航点距离太近，正常是仿地降高度航点，则取再下一个航点
continue;
}else{
circle_init_yaw = false;
wp_yaw_cd = cmd.content.location.get_bearing_to(next_cmd.content.location);
break;
}
}
}
}
// if no delay as well as not final waypoint set the waypoint as "fast"
AP_Mission::Mission_Command temp_cmd;
@ -1287,36 +1308,26 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) @@ -1287,36 +1308,26 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_NAV_SPLINE_WAYPOINT:
// 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 && wp_nav_index < 2){ // 第一个wp需要转向相关。两个航点距离太近，正常是仿地降高度航点，则取再下一个航点
// 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, "-b new rad:%d",wp_yaw_cd);
}
}else{
wp_yaw_cd = next_2_cmd.content.location.get_bearing_to(temp_cmd.content.location);
// gcs().send_text(MAV_SEVERITY_CRITICAL, "-a new rad:%d",wp_yaw_cd);
}
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 && wp_nav_index > 0){ // 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();
// fast_waypoint = true;
if(copter.wp_nav->get_fast_waypoint() == 2){
static float last_dist = 0;
static int32_t last_yaw_cd;
float wp_distance = cmd.content.location.get_distance(temp_cmd.content.location);
wp_yaw_cd = cmd.content.location.get_bearing_to(temp_cmd.content.location);
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;
if(wp_distance - last_dist > 0 && wp_nav_index > 0){ // 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:
@ -1712,7 +1723,6 @@ bool ModeAuto::verify_takeoff() @@ -1712,7 +1723,6 @@ bool ModeAuto::verify_takeoff()
}else{
return reached_wp_dest;
}
}
// verify_land - returns true if landing has been completed
@ -2044,7 +2054,7 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd @@ -2044,7 +2054,7 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd
return false;
}
copter.flow_measure.set_wp_index(cmd.index - copter.wp_nav->get_index_start()); // 修正测流起始航点
copter.flow_measure.set_wp_index(wp_nav_index - copter.wp_nav->get_index_start()); // 修正测流起始航点
copter.flow_measure.set_reach_flow_wp(true); //到达航点标志置1
if(copter.flow_measure.get_flow_update_state() < 2){
loiter_time = millis();

9
ArduCopter/mode_rtl.cpp
Unescape View File

@ -429,8 +429,13 @@ void ModeRTL::descent_run() @@ -429,8 +429,13 @@ void ModeRTL::descent_run()
if (tnow - last_1s > 1000) {
last_1s = tnow;
copter.Log_Write_Data(DATA_UPDOWN_SPEED,rtl_path.descent_target.alt);
copter.Log_Write_Data(DATA_UPDOWN_SPEED,copter.current_loc.alt);
// copter.Log_Write_Data(DATA_UPDOWN_SPEED,rtl_path.descent_target.alt);
// copter.Log_Write_Data(DATA_UPDOWN_SPEED,copter.current_loc.alt);
AP::logger().Write("RTES", "TimeUS,Stat,Dalt,Calt", "QBii",
AP_HAL::micros64(),
copter.updownStatus,
rtl_path.descent_target.alt,
copter.current_loc.alt);
if(labs(last_alt - copter.current_loc.alt) < 30){
timeout_count += 1;
}else{

2
ArduCopter/version.h
Unescape View File

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

2
d100h.sh
Unescape View File

@ -1,3 +1,3 @@ @@ -1,3 +1,3 @@
./waf configure --board d100h
./waf copter
cp ./build/d100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/d100h-v4.0.16-AVDv6.px4
cp ./build/d100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/编译/d100h-v4.0.17-RC5.px4

Write Preview
Loading…
Cancel
Save