Diff: setup.cpp

Revision:

141:725321fe2949

Parent:

139:b378528c05f2

Child:

143:53808e4e684c

--- a/setup.cpp	Mon Dec 06 11:37:55 2021 +0000
+++ b/setup.cpp	Fri Dec 10 10:43:50 2021 +0000
@@ -1,66 +1,66 @@
-//#include "global.hpp"
-//using namespace std;
-//
-//void setup()
-//{
-//    //sd.baud(115200);
-//    //sd.printf("\r\nFlight Start\r\n");
-//    //twelite.baud(38400);
-//    twelite.serial.printf("\r\nTelemetory Start\r\n");
-//    uint16_t reg = lsm.begin(lsm.G_SCALE_245DPS, lsm.A_SCALE_8G);
-//    //printf("%x\n", reg);
-//    if (!lps.init()){
-//        twelite.serial.printf("Failed to autodetect pressure sensor!\r\n");
-//        while (1);
-//    }
-//    lps.enableDefault();
-//    gps.Attach();
-//    wait_ms(100);
-//    
-//    SensorAlignmentAG << 1.0f << 0.0f << 0.0f
-//                      << 0.0f << 1.0f << 0.0f
-//                      << 0.0f << 0.0f << -1.0f;
-//    SensorAlignmentMAG << -1.0f <<  0.0f << 0.0f
-//                       <<  0.0f <<  1.0f << 0.0f
-//                       <<  0.0f <<  0.0f << -1.0f; 
-//    float magMin[3] =  {-392.590332, -85.194908, -155.781174};
-//    float magMax[3] =  {182.602386, 530.811523, 365.834625};
-//    magCalibrator.setExtremes(magMin,magMax);
-//    
-//    pitchPID.setSetPoint(0.0);
-//    pitchratePID.setSetPoint(0.0); 
-//    rollPID.setSetPoint(0.0); 
-//    rollratePID.setSetPoint(0.0); 
-//    pitchPID.setBias(0.0);
-//    pitchratePID.setBias(0.0); 
-//    rollPID.setBias(0.0); 
-//    rollratePID.setBias(0.0); 
-//    pitchPID.setOutputLimits(-1.0,1.0);
-//    pitchratePID.setOutputLimits(-1.0,1.0);
-//    rollPID.setOutputLimits(-1.0,1.0); 
-//    rollratePID.setOutputLimits(-1.0,1.0);
-//    pitchPID.setInputLimits(-M_PI,M_PI);
-//    pitchratePID.setInputLimits(-M_PI,M_PI);
-//    rollPID.setInputLimits(-M_PI,M_PI); 
-//    rollratePID.setInputLimits(-M_PI,M_PI);
-//    
-//    servoRight.period_us(15000.0);
-//    servoLeft.period_us(15000.0);
-//    servoThrust.period_us(15000.0);
-//    servoRight.pulsewidth_us(1500.0);
-//    servoLeft.pulsewidth_us(1500.0); 
-//    servoThrust.pulsewidth_us(1100.0);
-//    
-//    autopilot.set_dest(0.0f, 50.0f);
-//    autopilot.set_turn(0.0f, 50.0f, 50.0f);
-//    autopilot.set_alt(30.0f, 10.0f);
-//    autopilot.set_climb(3.0f*M_PI/180.0f, 0);
-//}
-//
-//void calibrate()
-//{
-//    while(1)
-//    {
-//        wait(1000);
-//    }
-//}
\ No newline at end of file
+#include "global.hpp"
+using namespace std;
+
+void setup()
+{
+    //sd.baud(115200);
+    //sd.printf("\r\nFlight Start\r\n");
+    //twelite.baud(38400);
+    twelite.serial.printf("\r\nTelemetory Start\r\n");
+    uint16_t reg = lsm.begin(lsm.G_SCALE_245DPS, lsm.A_SCALE_8G);
+    //printf("%x\n", reg);
+    if (!lps.init()){
+        twelite.serial.printf("Failed to autodetect pressure sensor!\r\n");
+        while (1);
+    }
+    lps.enableDefault();
+    gps.Attach();
+    wait_ms(100);
+    
+    SensorAlignmentAG << 1.0f, 0.0f, 0.0f,
+                         0.0f, 1.0f, 0.0f,
+                         0.0f, 0.0f, -1.0f;
+    SensorAlignmentMAG << -1.0f, 0.0f, 0.0f,
+                           0.0f, 1.0f, 0.0f,
+                           0.0f, 0.0f, -1.0f; 
+    float magMin[3] =  {-392.590332, -85.194908, -155.781174};
+    float magMax[3] =  {182.602386, 530.811523, 365.834625};
+    magCalibrator.setExtremes(magMin,magMax);
+    
+    pitchPID.setSetPoint(0.0f);
+    pitchratePID.setSetPoint(0.0f); 
+    rollPID.setSetPoint(0.0f); 
+    rollratePID.setSetPoint(0.0f); 
+    pitchPID.setBias(0.0f);
+    pitchratePID.setBias(0.0f); 
+    rollPID.setBias(0.0f); 
+    rollratePID.setBias(0.0f); 
+    pitchPID.setOutputLimits(-1.0f,1.0f);
+    pitchratePID.setOutputLimits(-1.0f,1.0f);
+    rollPID.setOutputLimits(-1.0f,1.0f); 
+    rollratePID.setOutputLimits(-1.0f,1.0f);
+    pitchPID.setInputLimits(-M_PI_F,M_PI_F);
+    pitchratePID.setInputLimits(-M_PI_F,M_PI_F);
+    rollPID.setInputLimits(-M_PI_F,M_PI_F); 
+    rollratePID.setInputLimits(-M_PI_F,M_PI_F);
+    
+    servoRight.period_us(15000.0f);
+    servoLeft.period_us(15000.0f);
+    servoThrust.period_us(15000.0f);
+    servoRight.pulsewidth_us(1500.0f);
+    servoLeft.pulsewidth_us(1500.0f); 
+    servoThrust.pulsewidth_us(1100.0f);
+    
+    autopilot.set_dest(0.0f, 50.0f);
+    autopilot.set_turn(0.0f, 50.0f, 50.0f);
+    autopilot.set_alt(30.0f, 10.0f);
+    autopilot.set_climb(3.0f*M_PI_F/180.0f, 0.0f);
+}
+
+void calibrate()
+{
+    while(1)
+    {
+        wait(1000);
+    }
+}
\ No newline at end of file