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

autopilot.cpp

Committer:
NaotoMorita
Date:
22 months ago
Revision:
143:53808e4e684c
Parent:
140:53dbdb207542

File content as of revision 143:53808e4e684c:

#include "global.hpp"
 
void level_flight()
{
    Vector3f vdot = calc_vdot();
    Vector3f pihat = eskf.getPihat();
    Vector3f vihat = eskf.getVihat();
    autopilot.update_val(euler, -palt, pihat, vihat, vdot);
    autopilot.level();
    autopilot.keep_alt();
    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
}
 
void point_guide()
{
    Vector3f vdot = calc_vdot();
    Vector3f pihat = eskf.getPihat();
    Vector3f vihat = eskf.getVihat();
    autopilot.update_val(euler, -palt, pihat, vihat, vdot);
    autopilot.guide();
    autopilot.keep_alt();
    autopilot.return_val(roll_obj, pitch_obj, dT_obj);  
}
 
void turning()
{
    Vector3f vdot = calc_vdot();
    Vector3f pihat = eskf.getPihat();
    Vector3f vihat = eskf.getVihat();
    autopilot.update_val(euler, -palt, pihat, vihat, vdot);
    autopilot.turn();
    autopilot.keep_alt();
    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
}

void climb()
{
    Vector3f vdot = calc_vdot();
    Vector3f pihat = eskf.getPihat();
    Vector3f vihat = eskf.getVihat();
    autopilot.update_val(euler, -palt, pihat, vihat, vdot);
    autopilot.level();
    autopilot.climb();
    autopilot.return_val(roll_obj, pitch_obj, dT_obj);
}

Vector3f calc_vdot()
{
    Vector3f m_vdot = eskf.calcDynAcc(acc);
    Vector3f vdot;
    vdot = m_vdot;
    return vdot;
}