control_sport.cak 21 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576
  1. #include "Sub.h"
  2. //#include "/AP_Math/AP_Math.h"
  3. extern mavlink_rov_state_monitoring_t rov_message;
  4. Quaternion attitude_desired_quat_;
  5. /*
  6. * control_althold.pde - init and run calls for althold, flight mode
  7. */
  8. // althold_init - initialise althold controller
  9. bool Sub::sport_init()
  10. {
  11. attitude_control.set_throttle_out(0.5 ,true, g.throttle_filt);
  12. //holding_depth = false;
  13. //检查是否有深度计存在
  14. if(!control_check_barometer()) {
  15. return false;
  16. }
  17. // initialize vertical speeds and leash lengths
  18. // sets the maximum speed up and down returned by position controller
  19. //get_pilot_speed_dn QGC PILOT 设置的值
  20. pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);//(-500,500)设置最大最小速度 和刹车长度
  21. pos_control.set_max_accel_z(g.pilot_accel_z);//设置最大加速度值 和刹车长度
  22. pos_control.relax_alt_hold_controllers();//目标高度为当前高度,目标速度和上次速度 当前速度
  23. //
  24. //pos_control.set_target_to_stopping_point_z();//刹车长度 根据当前速度估计目标深度
  25. pos_control.calc_leash_length_z();//刹车长度 12.19
  26. pos_control.set_alt_target(barometer.get_altitude()*100);//当前深度12.19
  27. holding_depth = true;
  28. ahrs.get_quat_body_to_ned(attitude_desired_quat_);//12.19
  29. last_roll = 0;
  30. last_pitch = 0;
  31. updowmgain =0.5;
  32. last_yaw = ahrs.yaw_sensor;
  33. last_input_ms = AP_HAL::millis();
  34. last_input_ms_stable = AP_HAL::millis();
  35. return true;
  36. }
  37. void Sub::handle_attitude_sport(){
  38. uint32_t tnow = AP_HAL::millis();
  39. // get pilot desired lean angles
  40. float target_roll, target_pitch, target_yaw;
  41. // Check if set_attitude_target_no_gps is valid
  42. if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
  43. Quaternion(
  44. set_attitude_target_no_gps.packet.q
  45. ).to_euler(
  46. target_roll,
  47. target_pitch,
  48. target_yaw
  49. );
  50. target_roll = 100 * degrees(target_roll);
  51. target_pitch = 100 * degrees(target_pitch);
  52. target_yaw = 100 * degrees(target_yaw);
  53. last_roll = target_roll;
  54. last_pitch = target_pitch;
  55. last_yaw = target_yaw;
  56. attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true);
  57. } else {
  58. // If we don't have a mavlink attitude target, we use the pilot's input instead
  59. //get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
  60. //get_pilot_desired_lean_angles((int16_t)(channel_lateral->norm_input()*5700), (int16_t)((0.5-channel_throttle->norm_input())*5700*2), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
  61. //get_pilot_desired_lean_angles(0, (int16_t)((0.5-channel_throttle->norm_input())*5700*2), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
  62. float throttle = 2*(0.5-channel_throttle->norm_input());
  63. float yaw = channel_yaw->norm_input();
  64. if (fabsf(throttle)>=fabsf(yaw))
  65. {
  66. yaw = 0.0;
  67. }else{
  68. throttle = 0.0;
  69. yaw = channel_yaw->get_control_in()*0.3;
  70. }
  71. get_pilot_desired_lean_angles(0, (int16_t)(throttle/2.0*5700), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
  72. target_yaw = get_pilot_desired_yaw_rate(yaw);//*0.6 变慢
  73. if (abs(target_roll) > 300 || abs(target_pitch) > 300) {
  74. //last_roll = ahrs.roll_sensor;
  75. //last_pitch = ahrs.pitch_sensor;
  76. //last_yaw = ahrs.yaw_sensor;
  77. ahrs.get_quat_body_to_ned(attitude_desired_quat_);
  78. last_input_ms = tnow;
  79. attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
  80. } else if (abs(target_yaw) > 300) {
  81. // if only yaw is being controlled, don't update pitch and roll
  82. attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, target_yaw);
  83. //last_yaw = ahrs.yaw_sensor;
  84. ahrs.get_quat_body_to_ned(attitude_desired_quat_);
  85. last_input_ms = tnow;
  86. } else if (tnow < last_input_ms + 250) {
  87. // just brake for a few mooments so we don't bounce
  88. last_yaw = ahrs.yaw_sensor;
  89. attitude_control.input_rate_bf_roll_pitch_yaw(0, 0, 0);
  90. } else {
  91. // Lock attitude
  92. //attitude_control.input_euler_angle_roll_pitch_yaw(last_roll, last_pitch, last_yaw, true);
  93. attitude_control.input_quaternion(attitude_desired_quat_);
  94. }
  95. }
  96. }
  97. void Sub::sport_run_alt(void){
  98. // When unarmed, disable motors and stabilization
  99. if (!motors.armed()) {
  100. motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
  101. // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
  102. attitude_control.set_throttle_out(0.5 ,true, g.throttle_filt);
  103. attitude_control.relax_attitude_controllers();
  104. pos_control.relax_alt_hold_controllers();
  105. pos_control.set_alt_target(barometer.get_altitude()*100);//当前深度
  106. updowmgain =0.5;
  107. last_roll = 0;
  108. last_pitch = 0;
  109. last_yaw = ahrs.yaw_sensor;
  110. holding_depth = false;
  111. ahrs.get_quat_body_to_ned(attitude_desired_quat_);//12.19
  112. return;
  113. }
  114. motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
  115. handle_attitude_sport();
  116. //pos_control.update_z_controller();//输出了set_throttle_out
  117. Matrix3f mat_body = ahrs.get_rotation_body_to_ned();
  118. Vector3f Zb = mat_body*Vector3f(0,0,1);
  119. Vector3f Ze = Vector3f(0,0,1);
  120. float thetaz = Zb*Ze;//计算有没有翻过来
  121. Matrix3f mat_half;
  122. if(mat_body.c.x<0.98 && mat_body.c.x>-0.98){//大约78.5度认为Z轴指向方向,小于78.5进入X轴指向方向
  123. float beta = 0.0;
  124. beta = safe_sqrt(1.0-mat_body.c.x*mat_body.c.x);
  125. if (thetaz<0)//翻过来
  126. {
  127. beta =-beta;
  128. }
  129. mat_half.a.x= mat_body.a.x/beta;
  130. mat_half.a.y=-mat_body.b.x/beta;
  131. mat_half.a.z=0.0;
  132. mat_half.b.x= mat_body.b.x/beta;
  133. mat_half.b.y= mat_body.a.x/beta;
  134. mat_half.b.z=0.0;
  135. mat_half.c.x= 0.0;
  136. mat_half.c.y= 0.0;
  137. mat_half.c.z=1.0;
  138. }
  139. else{
  140. Vector3f Xb = mat_body*Vector3f(0,0,1);
  141. Xb.z = 0.0;
  142. Vector3f Xe = Vector3f(1,0,0);
  143. float alphacos = Xb * Xe/Xb.length();
  144. float beta = safe_sqrt(1.0-alphacos*alphacos);
  145. if (Xb.y<0)
  146. {
  147. beta =-beta;
  148. }
  149. mat_half.a.x= alphacos;
  150. mat_half.a.y=-beta;
  151. mat_half.a.z=0.0;
  152. mat_half.b.x= beta;
  153. mat_half.b.y=alphacos;
  154. mat_half.b.z=0.0;
  155. mat_half.c.x= 0.0;
  156. mat_half.c.y= 0.0;
  157. mat_half.c.z=1.0;
  158. }
  159. // mat 地到half状态
  160. Matrix3f rc_vehicle_frame = mat_half*ahrs.get_rotation_body_to_ned().transposed();//rc_vehicle_frame(half)到body
  161. float forward = channel_forward->norm_input();
  162. float lateral =channel_lateral->norm_input();
  163. if (fabsf(forward)>=fabsf(lateral))
  164. {
  165. lateral = 0.0;
  166. }else{
  167. forward = 0.0;
  168. }
  169. Vector3f rc_throttle = Vector3f(getgainf(forward), getgainf(lateral), 2*(gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5));//手柄的值
  170. Vector3f rc_throttle_vehicle_frame = rc_vehicle_frame*rc_throttle;//手柄从half系到body系
  171. static uint16_t count2 = 0;
  172. count2++;
  173. if (count2>200)
  174. {
  175. count2 = 0;
  176. gcs().send_text(MAV_SEVERITY_INFO, " rc_throttle %f %f %f\n",rc_throttle.x,rc_throttle.y,rc_throttle.z);
  177. // gcs().send_text(MAV_SEVERITY_INFO, " mata %f %f %f\n",mat_half.a.x,mat_half.a.y,mat_half.a.z);
  178. // gcs().send_text(MAV_SEVERITY_INFO, " matb %f %f %f\n",mat_half.b.x,mat_half.b.y,mat_half.b.z);
  179. // gcs().send_text(MAV_SEVERITY_INFO, " matc %f %f %f\n",mat_half.c.x,mat_half.c.y,mat_half.c.z);
  180. // Matrix3f matx = ahrs.get_rotation_body_to_ned();
  181. // gcs().send_text(MAV_SEVERITY_INFO, " ahrsa %f %f %f\n",matx.a.x,matx.a.y,matx.a.z);
  182. // gcs().send_text(MAV_SEVERITY_INFO, " ahrsb %f %f %f\n",matx.b.x,matx.b.y,matx.b.z);
  183. // gcs().send_text(MAV_SEVERITY_INFO, " ahrsc %f %f %f\n",matx.c.x,matx.c.y,matx.c.z);
  184. }
  185. // Read the output of the z controller and rotate it so it always points up
  186. //将Z轴输出变到机体坐标系
  187. //Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, motors.get_throttle_in_bidirectional());
  188. float z_throttle = pos_control.update_z_controller_f();//输出了set_throttle_out
  189. Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, z_throttle);
  190. //手柄控制和Z轴输出的叠加
  191. motors.set_forward(-throttle_vehicle_frame.x +rc_throttle_vehicle_frame.x);
  192. motors.set_lateral(-throttle_vehicle_frame.y +rc_throttle_vehicle_frame.y);//*0.6 变慢
  193. motors.set_throttle(throttle_vehicle_frame.z +rc_throttle_vehicle_frame.z/2+0.5);
  194. rov_message.pressure_level = int(PressLevel);
  195. static uint16_t count3 = 0;
  196. count3++;
  197. if (count3>200)
  198. {
  199. count3 = 0;
  200. gcs().send_text(MAV_SEVERITY_INFO, " rc_frame %f %f %f\n",z_throttle,throttle_vehicle_frame.z);
  201. // gcs().send_text(MAV_SEVERITY_INFO, " rc_frame %f %f %f\n",rc_vehicle_frame.a.x,rc_vehicle_frame.a.y,rc_vehicle_frame.a.z);
  202. // gcs().send_text(MAV_SEVERITY_INFO, " rc_frame %f %f %f\n",rc_vehicle_frame.b.x,rc_vehicle_frame.b.y,rc_vehicle_frame.b.z);
  203. // gcs().send_text(MAV_SEVERITY_INFO, " rc_frame %f %f %f\n",rc_vehicle_frame.c.x,rc_vehicle_frame.c.y,rc_vehicle_frame.c.z);
  204. //gcs().send_text(MAV_SEVERITY_INFO, " rc_throttle %f %f %f\n",rc_throttle.x,rc_throttle.y,rc_throttle.z);
  205. // gcs().send_text(MAV_SEVERITY_INFO, " rc %f %f %f\n",rc_throttle_vehicle_frame.x,rc_throttle_vehicle_frame.y,rc_throttle_vehicle_frame.z);
  206. }
  207. Vector3f earth_frame_rc_inputs = Vector3f(0.0, 0.0, 2*(gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5));//去掉侧移影响
  208. if (fabsf(earth_frame_rc_inputs.z) > 0.05f) { // Throttle input above 5%
  209. // reset z targets to current values
  210. holding_depth = false;
  211. pos_control.relax_alt_hold_controllers();
  212. } else { // hold z
  213. if (ap.at_surface) { //最大油门不能向上动
  214. pos_control.set_alt_target(g.surface_depth - 10.0f); // set target to 5cm below surface level
  215. holding_depth = true;
  216. } else if (ap.at_bottom) {//最大油门不能向下动
  217. //pos_control.set_alt_target(inertial_nav.get_altitude() + 20.0f); // set target to 10 cm above bottom
  218. pos_control.set_alt_target(barometer.get_altitude()*100 +20);//cm
  219. holding_depth = true;
  220. } else if (!holding_depth) {
  221. //pos_control.set_target_to_stopping_point_z();//有对位置的付值
  222. pos_control.calc_leash_length_z();//刹车长度 12.19
  223. pos_control.set_alt_target(barometer.get_altitude()*100);//cm 当前深度
  224. holding_depth = true;
  225. }
  226. }
  227. static bool lastdepth = holding_depth;
  228. if(lastdepth !=holding_depth){
  229. lastdepth = holding_depth;
  230. gcs().send_text(MAV_SEVERITY_INFO, " get_alt_target %d %f \n",(int)holding_depth,motors.get_throttle());
  231. }
  232. static uint16_t count = 0;
  233. count++;
  234. if (count>200)
  235. {
  236. count = 0;
  237. gcs().send_text(MAV_SEVERITY_INFO, " altitude %f %f %f \n",pos_control.get_alt_target(),barometer.get_altitude()*100,motors.get_throttle());
  238. gcs().send_text(MAV_SEVERITY_INFO, " surface %d %d\n",(int)ap.at_surface,(int)ap.at_bottom);
  239. }
  240. }
  241. void Sub::altcontrol_ff(){
  242. float speedclimb =SRV_Channels::srv_channel(15)->get_output_min()/10;//test
  243. float target_climb_rate = ((altdelay(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5)*speedclimb);
  244. target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
  245. Vector3f earth_frame_rc_inputs1 = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), 0.0, 2*(gaindelay2(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5));//去掉侧移影响
  246. rov_message.pressure_level = int(PressLevel);
  247. if (fabsf(earth_frame_rc_inputs1.z) > 0.05f) { // Throttle input above 5%
  248. // reset z targets to current values
  249. pos_control.relax_alt_hold_controllers();
  250. } else {
  251. if(ap.at_surface && target_climb_rate>0.0){
  252. pos_control.relax_part_hold_controllers();
  253. pos_control.set_alt_target(g.surface_depth - 10.0f); // set target to 10cm below surface level
  254. } else if (ap.at_bottom && target_climb_rate<0.0) {
  255. pos_control.relax_part_hold_controllers(); // clear ff control
  256. pos_control.set_alt_target(barometer.get_altitude()*100); // set target to current
  257. } else {
  258. pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);//前馈控制 生成目标深度
  259. }
  260. }
  261. float z_throttle = pos_control.update_z_controller_f();//输出了set_throttle_out
  262. Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, z_throttle);//转到机体系
  263. motors.set_throttle(throttle_vehicle_frame.z+gaindelay2(1.0-(float)PressLevel_f*0.1,updowmgain));//z轴付值
  264. static uint16_t count = 0;
  265. count++;
  266. if (count>300)
  267. {
  268. count = 0;
  269. gcs().send_text(MAV_SEVERITY_INFO, "climb_rate %f %f %f %f\n",target_climb_rate,throttle_vehicle_frame.z,updowmgain,speedclimb);
  270. gcs().send_text(MAV_SEVERITY_INFO, " altitude %f %f \n",pos_control.get_alt_target(),barometer.get_altitude()*100);
  271. }
  272. // 前进侧移付值
  273. float forward = channel_forward->norm_input();
  274. float lateral =channel_lateral->norm_input();
  275. if (fabsf(forward)>=fabsf(lateral))
  276. {
  277. lateral = 0.0;
  278. }else{
  279. forward = 0.0;
  280. }
  281. motors.set_forward(constrain_float(-throttle_vehicle_frame.x + getgainf(forward),-0.8,0.8));// *0.6 变慢
  282. motors.set_lateral(constrain_float(-throttle_vehicle_frame.y + getgainf(lateral),-0.8,0.8));// *0.6 变慢
  283. }
  284. void Sub::altcontrol(){
  285. float z_throttle = pos_control.update_z_controller_f();//输出了set_throttle_out
  286. // Read the output of the z controller and rotate it so it always points up
  287. //变到机体坐标系
  288. Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, z_throttle);
  289. // Output the Z controller + pilot input to all motors.
  290. motors.set_throttle(throttle_vehicle_frame.z + gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain));
  291. static uint16_t j=0;
  292. j++;
  293. if(j>300)
  294. {
  295. gcs().send_text(MAV_SEVERITY_INFO, " sportthrottle %f %f \n",z_throttle,throttle_vehicle_frame.z);
  296. gcs().send_text(MAV_SEVERITY_INFO, " throttle %f %f\n",gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain),motors.get_throttle());
  297. j=0;
  298. }
  299. rov_message.pressure_level = int(PressLevel);
  300. float forward = channel_forward->norm_input();
  301. float lateral =channel_lateral->norm_input();
  302. if (fabsf(forward)>=fabsf(lateral))
  303. {
  304. lateral = 0.0;
  305. }else{
  306. forward = 0.0;
  307. }
  308. motors.set_forward(constrain_float(-throttle_vehicle_frame.x + getgainf(forward),-0.8,0.8));//*0.6 变慢
  309. motors.set_lateral(constrain_float(-throttle_vehicle_frame.y + getgainf(lateral),-0.8,0.8));//*0.6 变慢
  310. // We rotate the RC inputs to the earth frame to check if the user is giving an input that would change the depth.
  311. //Vector3f earth_frame_rc_inputs = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), channel_lateral->norm_input(), (2.0f*(-0.5f+channel_throttle->norm_input())));
  312. //Vector3f earth_frame_rc_inputs = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), channel_lateral->norm_input(), (2.0*(0.5-PressLevel_f*0.1)));
  313. Vector3f earth_frame_rc_inputs1 = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), 0.0, 2*(gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5));//去掉侧移影响
  314. //Vector3f earth_frame_rc_inputs2 = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), 0.0, motors.get_throttle_bidirectional());//去掉侧移影响
  315. if (fabsf(earth_frame_rc_inputs1.z) > 0.05f) { // Throttle input above 5%
  316. // reset z targets to current values
  317. holding_depth = false;
  318. pos_control.relax_alt_hold_controllers();
  319. //pos_control.set_alt_target(barometer.get_altitude()*100);//当前深度
  320. } else { // hold z
  321. if (ap.at_surface) { //最大油门不能向上动
  322. pos_control.set_alt_target(g.surface_depth - 10.0f); // set target to 5cm below surface level
  323. holding_depth = true;
  324. } else if (ap.at_bottom) {//最大油门不能向下动
  325. //pos_control.set_alt_target(inertial_nav.get_altitude() + 20.0f); // set target to 10 cm above bottom
  326. pos_control.set_alt_target(barometer.get_altitude()*100);//cm
  327. holding_depth = true;
  328. } else if (!holding_depth) {
  329. pos_control.calc_leash_length_z();//刹车长度 12.19
  330. pos_control.set_alt_target(barometer.get_altitude()*100);//cm 当前深度
  331. holding_depth = true;
  332. }
  333. }
  334. static bool lastdepth = holding_depth;
  335. if(lastdepth !=holding_depth){
  336. lastdepth = holding_depth;
  337. gcs().send_text(MAV_SEVERITY_INFO, " surface %d %d\n",(int)ap.at_surface,(int)ap.at_bottom);
  338. gcs().send_text(MAV_SEVERITY_INFO, " get_alt_target %d %f %f \n",(int)holding_depth,gaindelay(1.0-(float)PressLevel_f*0.1,updowmgain),motors.get_throttle());
  339. }
  340. static uint16_t count = 0;
  341. count++;
  342. if (count>300)
  343. {
  344. count = 0;
  345. gcs().send_text(MAV_SEVERITY_INFO, " altitude %f %f \n",pos_control.get_alt_target(),barometer.get_altitude()*100);
  346. }
  347. }
  348. void Sub::ratecontrol(){
  349. float speedclimb =SRV_Channels::srv_channel(15)->get_output_min()/10;//test
  350. float target_climb_rate = ((altdelay(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5)*speedclimb);
  351. target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
  352. uint32_t tnow = AP_HAL::millis();
  353. float alt = 0.0;
  354. static uint32_t lasttime = tnow;
  355. static uint8_t alt_flag = 0;
  356. bool alt_flag2 = false;
  357. Vector3f earth_frame_rc_inputs1 = ahrs.get_rotation_body_to_ned() * Vector3f(channel_forward->norm_input(), 0.0, 2*(gaindelay2(1.0-(float)PressLevel_f*0.1,updowmgain)-0.5));//压力分级
  358. rov_message.pressure_level = int(PressLevel);
  359. if (fabsf(earth_frame_rc_inputs1.z) > 0.05f) { // Throttle input above 5%
  360. // reset z targets to current values
  361. pos_control.relax_alt_hold_controllers();
  362. holding_depth = false;
  363. alt_flag = 0;
  364. } else { //没有压力分级的时候
  365. if(ap.at_surface ){
  366. pos_control.set_alt_target(g.surface_depth - 10.0f); // set target to 10cm below surface level
  367. holding_depth = true;
  368. } else if (ap.at_bottom ) {
  369. pos_control.set_alt_target(barometer.get_altitude()*100+10); // set target to current
  370. holding_depth = true;
  371. } else {
  372. if (!holding_depth)
  373. {
  374. holding_depth = true;
  375. pos_control.set_alt_target(barometer.get_altitude()*100); // set target to current
  376. }
  377. }
  378. if (fabsf(target_climb_rate)>0.0 )
  379. {
  380. alt_flag = 1;
  381. alt =pos_control.run_z_controller_wangdan(target_climb_rate);
  382. lasttime = tnow;
  383. alt_flag2 =true;
  384. }else if(tnow-lasttime<300){
  385. alt_flag2 =true;
  386. alt =pos_control.run_z_controller_wangdan(0.0);
  387. }else{
  388. if (alt_flag ==1)
  389. {
  390. alt_flag = 0;
  391. pos_control.set_alt_target(barometer.get_altitude()*100); // set target to current
  392. }
  393. }
  394. }
  395. if (!alt_flag2)
  396. {
  397. alt=pos_control.update_z_controller_f();
  398. }
  399. Vector3f throttle_vehicle_frame = ahrs.get_rotation_body_to_ned().transposed() * Vector3f(0, 0, alt);
  400. motors.set_throttle(throttle_vehicle_frame.z + gaindelay2(1.0-(float)PressLevel_f*0.1,updowmgain));
  401. // 前进侧移付值
  402. float forward = channel_forward->norm_input();
  403. float lateral =channel_lateral->norm_input();
  404. if (fabsf(forward)>=fabsf(lateral))
  405. {
  406. lateral = 0.0;
  407. }else{
  408. forward = 0.0;
  409. }
  410. motors.set_forward(constrain_float(-throttle_vehicle_frame.x + getgainf(forward),-0.8,0.8));// *0.6 变慢
  411. motors.set_lateral(constrain_float(-throttle_vehicle_frame.y + getgainf(lateral),-0.8,0.8));// *0.6 变慢
  412. static uint16_t count = 0;
  413. count++;
  414. if (count>300)
  415. {
  416. count = 0;
  417. gcs().send_text(MAV_SEVERITY_INFO, " altitude %f %f %f \n",pos_control.get_alt_target(),barometer.get_altitude()*100,throttle_vehicle_frame.z );
  418. gcs().send_text(MAV_SEVERITY_INFO, " throttle_lower %d %f %f\n",(int)motors.limit.throttle_lower,motors.get_throttle_filter().get_cutoff_freq(),pos_control.alt_rate.get());
  419. }
  420. }
  421. // althold_run - runs the althold controller
  422. // should be called at 100hz or more
  423. void Sub::sport_run()
  424. {
  425. // When unarmed, disable motors and stabilization
  426. if (!motors.armed()) {
  427. motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
  428. // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
  429. attitude_control.set_throttle_out(0.5 ,true, g.throttle_filt);
  430. attitude_control.relax_attitude_controllers();
  431. pos_control.relax_alt_hold_controllers();
  432. //pos_control.set_alt_target(barometer.get_altitude()*100);//当前深度
  433. updowmgain =0.5;
  434. last_roll = 0;
  435. last_pitch = 0;
  436. last_yaw = ahrs.yaw_sensor;
  437. holding_depth = false;
  438. ahrs.get_quat_body_to_ned(attitude_desired_quat_);//12.19
  439. return;
  440. }
  441. // Vehicle is armed, motors are free to run
  442. motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
  443. handle_attitude_sport();
  444. //----------------------------------------速度前馈--------------------------
  445. //altcontrol_ff();
  446. //-----------------------------------------位置控制----------------------------------------
  447. /*
  448. altcontrol();
  449. */
  450. //-----------------------速度给定位置保持
  451. ratecontrol();
  452. }