HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

Eigen Revision

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

setup.cpp

Committer:
NaotoMorita
Date:
2021-10-29
Revision:
93:b827f78a717a
Parent:
88:be349faa1976
Child:
95:43717535c354

File content as of revision 93:b827f78a717a:

#include "global.hpp"
using namespace std;

void setup()
{
    sd.baud(115200);
    sd.printf("\r\nFlight Start\r\n");
    twelite.baud(38400);
    twelite.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.printf("Failed to autodetect pressure sensor!\r\n");
        while (1);
    }
    lps.enableDefault();
    wait_ms(100);
    
    SensorAlignment << 1.0f << 0.0f << 0.0f
                    << 0.0f << 1.0f << 0.0f
                    << 0.0f << 0.0f << -1.0f; 
    
    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);
}

void calibrate()
{
    while(1)
    {
        wait(1000);
    }
}