Revision 121:2523eef96b36, committed 2021-11-19

Comitter:

osaka

Date:

Fri Nov 19 07:56:16 2021 +0000

Parent:

120:a0da2ce20a8e

Child:

124:7d6b1b62483b

Commit message:

keep altitude and velocity

Changed in this revision

--- a/Autopilot.lib	Thu Nov 18 10:10:18 2021 +0000
+++ b/Autopilot.lib	Fri Nov 19 07:56:16 2021 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/HAPSRG/code/Autopilot/#1117ad251fef
+https://os.mbed.com/teams/HAPSRG/code/Autopilot/#988905aed916

--- a/autopilot.cpp	Thu Nov 18 10:10:18 2021 +0000
+++ b/autopilot.cpp	Fri Nov 19 07:56:16 2021 +0000
@@ -2,21 +2,24 @@
  
 void level_flight()
 {
-    autopilot.update_val(rpy, palt, pi);
+    autopilot.update_val(rpy, palt, pi, vi);
     autopilot.level();
-    autopilot.return_val(roll_obj, pitch_obj, alt_obj);
+    autopilot.keep_alt();
+    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
 }
  
 void point_guide()
 {
-    autopilot.update_val(rpy, palt, pi);
+    autopilot.update_val(rpy, palt, pi, vi);
     autopilot.guide();
-    autopilot.return_val(roll_obj, pitch_obj, alt_obj);  
+    autopilot.keep_alt();
+    autopilot.return_val(roll_obj, pitch_obj, dT_obj);  
 }
  
 void turning()
 {
-    autopilot.update_val(rpy, palt, pi);
+    autopilot.update_val(rpy, palt, pi, vi);
     autopilot.turn();
-    autopilot.return_val(roll_obj, pitch_obj, alt_obj);
+    autopilot.keep_alt();
+    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
 }
\ No newline at end of file

--- a/global.cpp	Thu Nov 18 10:10:18 2021 +0000
+++ b/global.cpp	Fri Nov 19 07:56:16 2021 +0000
@@ -31,10 +31,7 @@
 Autopilot autopilot;
 float roll_obj;
 float pitch_obj;
-float alt_obj;
-Vector3 destination(100.0f, 100.0f, 0.0f);
-Vector3 turn_center(100.0f, 100.0f, 0.0f);
-float turn_radius = 50.0f;
+float dT_obj;
 
 float rc[16];
 int loop_count = 0;

--- a/global.hpp	Thu Nov 18 10:10:18 2021 +0000
+++ b/global.hpp	Fri Nov 19 07:56:16 2021 +0000
@@ -82,10 +82,7 @@
 extern Autopilot autopilot;
 extern float roll_obj;
 extern float pitch_obj;
-extern float alt_obj;
-extern Vector3 destination;
-extern Vector3 turn_center;
-extern float turn_radius;
+extern float dT_obj;
 
 extern float rc[16];
 extern int loop_count;

--- a/servo.cpp	Thu Nov 18 10:10:18 2021 +0000
+++ b/servo.cpp	Fri Nov 19 07:56:16 2021 +0000
@@ -21,12 +21,16 @@
     rollPID.setProcessValue(rpy.x);
     rollratePID.setProcessValue(gyro.x);
     
-    if (true)
+    dT = rc[2];
+    
+    if (rc[6] > 0.0f)
     {
         //level_flight();
         //point_guide();
         turning();
         rollPID.setSetPoint(roll_obj);
+        pitchPID.setSetPoint(pitch_obj);
+        dT += dT_obj;
     }
     
     //舵角計算
@@ -37,8 +41,6 @@
         de = (pitchPID.compute()+pitchratePID.compute())+(rc[0]-rc[1])/2.0f;
         da = (rollPID.compute()+rollratePID.compute())+(rc[0]+rc[1])/2.0f;
     }
-
-    dT = rc[2];
     
     scaledServoOut[0]=de+da;
     scaledServoOut[1]=-de+da;

--- a/setup.cpp	Thu Nov 18 10:10:18 2021 +0000
+++ b/setup.cpp	Fri Nov 19 07:56:16 2021 +0000
@@ -52,8 +52,9 @@
     servoLeft.pulsewidth_us(1500.0); 
     servoThrust.pulsewidth_us(1100.0);
     
-    autopilot.set_dest(destination.x, destination.y);
-    autopilot.set_turn(turn_center.x, turn_center.y, turn_radius);
+    autopilot.set_dest(100.0f, 100.0f);
+    autopilot.set_turn(100.0f, 100.0f, 50.0f);
+    autopilot.set_alt(30.0f, 10.0f);
 }
 
 void calibrate()