diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 6755f1204f..f303706586 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -954,12 +954,27 @@ void ModeAuto::circle_run() if(g.zr_8_circle > 0){ // 起飞到高度绕8字 int32_t initial_armed_bearing = wp_yaw_cd; + static autopilot_yaw_mode last_yaw_mode; if(circle_init_yaw == false){ // 如果需要转向到下一个航点 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{ - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), initial_armed_bearing ,true); + + if(wp_nav->set_wp_destination(last_loc)) { + if (auto_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{ + + circle_init_yaw = true; + gcs().send_text(MAV_SEVERITY_INFO, "--- circle turn false ---"); + } + return; } }else{ // 转向完成后,开始绕8字 diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 83ca9d9def..47e6075fdc 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,7 +6,7 @@ #include "ap_version.h" -#define THISFIRMWARE "ZRUAV v4.0.17-RC2" //"ArduCopter V4.0.0" +#define THISFIRMWARE "ZRUAV v4.0.17-RC3" //"ArduCopter V4.0.0" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,0,17,FIRMWARE_VERSION_TYPE_OFFICIAL diff --git a/hrs100h.sh b/hrs100h.sh index 8fdc28e263..4bd82c5364 100755 --- a/hrs100h.sh +++ b/hrs100h.sh @@ -1,3 +1,3 @@ ./waf configure --board rs100h ./waf copter -cp ./build/rs100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/rs100h-v4.0.16.px4 +cp ./build/rs100h/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/rs100h-v4.0.17-RC3.px4 diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index b133ef5aab..7bd4f4df9a 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -365,6 +365,50 @@ void AC_Circle::update() _pos_control.update_xy_controller(); } + +/// update - update circle controller +void AC_Circle::turn_yaw(const Vector3f& center) +{ + // calculate dt + float dt = _pos_control.time_since_last_xy_update(); + if (dt >= 0.2f) { + dt = 0.0f; + } + + // ramp angular velocity to maximum + if (_angular_vel < _angular_vel_max) { + _angular_vel += fabsf(_angular_accel) * dt; + _angular_vel = MIN(_angular_vel, _angular_vel_max); + } + if (_angular_vel > _angular_vel_max) { + _angular_vel -= fabsf(_angular_accel) * dt; + _angular_vel = MAX(_angular_vel, _angular_vel_max); + } + + // update the target angle and total angle traveled + float angle_change = _angular_vel * dt; + _angle += angle_change; + _angle = wrap_PI(_angle); + _angle_total += angle_change; + + // set target position to center + Vector3f target; + target.x = center.x; + target.y = center.y; + target.z = _pos_control.get_alt_target(); + + // update position controller target + _pos_control.set_xy_target(target.x, target.y); + + // heading is same as _angle but converted to centi-degrees + _yaw = _angle * DEGX100; + + + // update position controller + _pos_control.update_xy_controller(); +} + + // get_closest_point_on_circle - returns closest point on the circle // circle's center should already have been set // closest point on the circle will be placed in result diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index f648a81252..34d4d824af 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/libraries/AC_WPNav/AC_Circle.h @@ -80,6 +80,7 @@ public: bool run_8_circle(bool reached); uint8_t run_rtl_8_circle(uint8_t *step); + void turn_yaw(const Vector3f& center); static const struct AP_Param::GroupInfo var_info[]; private: diff --git a/rs100.sh b/rs100.sh index e8c14b1b5d..3884294df1 100755 --- a/rs100.sh +++ b/rs100.sh @@ -1,3 +1,3 @@ ./waf configure --board rs100 ./waf copter -cp ./build/rs100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/rs100-v4.0.16.px4 +cp ./build/rs100/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/rs100-v4.0.17-RC3.px4