API tools faq
paste
enhering

Untitled

enhering
Sep 8th, 2017
158
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 14.76 KB | None | 0 0
raw download clone embed print report
  1. #ifndef __COM_H_
  2. #define __COM_H_
  3.  
  4.   #include "ModuleBase.h"
  5.   #include "UART.h"
  6.  
  7.   #define BOARD_BUILD     BUILD_NUMBER_STR
  8.  
  9.   #define NUMBER_BUFFER_SIZE 15
  10.  
  11.   enum COMStates {
  12.     COM_TROUBLESHOOT,
  13.     COM_INIT,
  14.     COM_BUILD_MODULE_CAPACITY_MATRIX,
  15.     COM_MANAGE_SENSORS,
  16.     COM_MANAGE_GPSS,
  17.     COM_MANAGE_KALMAN,
  18.     COM_MANAGE_GUIDANCE,
  19.     COM_MANAGE_NAVIGATION,
  20.     COM_MANAGE_CONTROL,
  21.     COM_MANAGE_SERVOS,
  22.     COM_MANAGE_POWER,
  23.     COM_ANSWER_UART,
  24.     COM_VERIFY_FLAGS,
  25.     COM_EVALUATE_STATUS,
  26.     COM_BROADCAST_TELEMETRY,
  27.     COM_MANAGE_HEARTBEAT
  28.   };
  29.  
  30.   ///
  31.   /// Communications module (COM) main class.
  32.   ///
  33.   /// The COM module is responsible for interrogating and/or delivering information
  34.   /// to all the other modules of the system via the main SPI bus. COM is the SPI master.
  35.   /// It needs physical access to the other modules' SS lines. The code calls all the slots
  36.   /// in sequence, asking them to send an identification byte. If a module is present
  37.   /// in the slot, it is identified and placed in the correct part of the operation
  38.   /// sequence. After identification, the main sequence is started. First the data source
  39.   /// modules are called, like sensor and GPS modules.
  40.   /// Then the data is sent to and from decision taking modules, such as the flight control one.
  41.   /// Finally the servo control module is called to send the new positions of the servos.
  42.   /// Power supply modules are also interrogated to inform power status to the system.
  43.   ///
  44.  
  45.   class COM : public ModuleBase {
  46.     public:
  47.       COM();
  48.       ~COM();
  49.  
  50.       void Run();
  51.  
  52.       void CustomSetup();
  53.       void CustomLoop();
  54.  
  55.       bool Init();               ///< Inits SPI, UART and other stuff.
  56.       bool BuildModuleCapacityMatrix();    ///< Identifies the connected modules and saves the information in the m_aeModuleIdentities array.
  57.       bool ManagePower();
  58.       bool ManageSensors();
  59.       bool ManageGPSs();
  60.       bool ManageKalman();
  61.       bool ManageGuidance();
  62.       bool ManageNavigation();
  63.       bool ManageControl();
  64.       bool ManageServos();
  65.       bool AnswerUART();         ///< Answers any UART request, sending back information or executing commands.
  66.       bool VerifyFlags();        ///< Verifies if an flags were set as requested by UART commands.
  67.       bool EvaluateStatus();     ///< Evaluates system status based on all modules status' information.
  68.       bool BroadcastTelemetry(); ///< Broadcasts telemetry via UART.
  69.       bool ManageHeartbeat();    ///< Sends heart beat to redundant COM module
  70.  
  71.       void TroubleShoot();        ///< Handle errors
  72.      
  73.       void ReadAndFuse(uint8_t, uint8_t, uint8_t, uint8_t);
  74.       float FuseValues(float, float, float, float, float &);
  75.  
  76.       void SetupSlotsSS();       ///< Sets all the GPIOs that controls the SS lines of each slot to OUTPUT and HIGH.
  77.       char * FormatFreeRAM(char *);  ///< Formats a char array with some text and free RAM size.
  78.       void ConvertAndConcatenate(DataType, char *);
  79.       char * FormatSlotOccupation(char *);               ///< Formats a string with slot occupation information.
  80.       void FormatLongAndSend(int32_t);
  81.  
  82.       void SendNextByteWrapper();     ///< Wraps the UART SendNextByte method so it can be accessed by COMModule.cpp.
  83.       void ReceiveNextByteWrapper();  ///< Wraps the UART ReceiveNextByte method so it can be accessed by COMModule.cpp. Also verifies if the reception was complete and, if so, sets a flag.
  84.  
  85.       void Echo(char *);  ///< Echoes wrong commands back to UART.
  86.  
  87.       //char * FloatToCharPtr(float, int, char *, uint8_t);
  88.  
  89.     private:
  90.       UART m_cUART;
  91.  
  92.       uint8_t m_nError;
  93.  
  94.       volatile bool m_bUARTMessageReady;
  95.       char m_achReceivedUARTMessage[UART_TRANSFER_BUFFER_SIZE];
  96.  
  97.       float m_afDataArray[MODULE_SELECT_TOTAL_OPTIONS];
  98.  
  99.       uint32_t m_nCycleCount;
  100.  
  101.       volatile uint8_t m_anSlotsStatuses[NUM_SLOTS+1];
  102.       volatile uint32_t m_anCapacitiesPerSlot[NUM_SLOTS+1];
  103.       volatile uint32_t m_anSlotListPerCapacity[TOTAL_CAPACITIES];
  104.  
  105.       volatile SystemStatus m_eSystemStatus;
  106.  
  107.       volatile bool m_bSendSPIData;
  108.  
  109.       volatile COMStates        m_eCOMState;
  110.  
  111.       volatile bool   m_bRestart;
  112.       volatile bool   m_bIdentifyModulesAgain;
  113.       volatile bool   m_bUARTMessageBeingProcessed;
  114.   };
  115.  
  116. #endif
  117.  
  118. #include "COM.h"
  119.  
  120. ///
  121. /// YAUVeC
  122. /// Brief description. Detailed stuff.
  123. ///
  124.  
  125. COM::COM() {
  126.   m_nError = 0;
  127.   m_nCycleCount = 0;
  128.  
  129.   m_eSystemStatus = STATUS_NORMAL;
  130.  
  131.   m_bUARTMessageReady = false;
  132.  
  133.   m_bSendSPIData = false;
  134.  
  135.   m_cBlink.SetLEDGPIO(7);
  136.  
  137.   m_eCOMState = COM_INIT;
  138.  
  139.   m_bRestart = false;
  140.   m_bIdentifyModulesAgain = false;
  141.   m_bUARTMessageBeingProcessed = true;
  142. }
  143.  
  144. COM::~COM() {
  145. }
  146.  
  147. void COM::Run() {
  148.  
  149.   CustomSetup();
  150.  
  151.   while(1) {
  152.     m_cBlink.ManageLED();
  153.  
  154.     CustomLoop();  
  155.   }
  156. }
  157.  
  158. void COM::CustomSetup() {
  159.   SetAsMasterSPI();
  160.  
  161.   m_cUART.Init();
  162.  
  163.   m_cBlink.SetLEDGPIO(7);
  164.   m_cBlink.SetLEDOnCount(10);
  165.   m_cBlink.SetLEDOffCount(100);
  166.   m_cBlink.SetAttentionLEDOnCount(100);
  167.   m_cBlink.SetAttentionLEDOffCount(100);
  168.  
  169.   m_cBlink.BlinkLED();
  170.  
  171.   SetupSlotsSS();
  172. }
  173.  
  174. void COM::CustomLoop() {
  175.  
  176.   m_nCycleCount++;
  177.  
  178.   switch (m_eCOMState) {
  179.     case COM_INIT:                
  180.       m_eCOMState = Init() ? COM_BUILD_MODULE_CAPACITY_MATRIX : COM_TROUBLESHOOT;
  181.       break;
  182.    
  183.     case COM_BUILD_MODULE_CAPACITY_MATRIX:
  184.       m_eCOMState = BuildModuleCapacityMatrix() ? COM_MANAGE_SENSORS : COM_TROUBLESHOOT;      
  185.       break;
  186.  
  187.     case COM_MANAGE_POWER:
  188.       m_eCOMState = ManagePower() ? COM_MANAGE_SENSORS : COM_ANSWER_UART;
  189.       break;
  190.    
  191.     case COM_MANAGE_SENSORS:
  192.       m_eCOMState = ManageSensors() ? COM_MANAGE_GPSS : COM_TROUBLESHOOT;      
  193.       break;
  194.  
  195.     case COM_MANAGE_GPSS:
  196.       m_eCOMState = ManageGPSs() ? COM_MANAGE_KALMAN : COM_TROUBLESHOOT;
  197.       break;
  198.  
  199.     case COM_MANAGE_KALMAN:
  200.       m_eCOMState = ManageKalman() ? COM_MANAGE_GUIDANCE : COM_TROUBLESHOOT;
  201.       break;
  202.  
  203.     case COM_MANAGE_GUIDANCE:
  204.       m_eCOMState = ManageGuidance() ? COM_MANAGE_NAVIGATION : COM_TROUBLESHOOT;
  205.       break;
  206.  
  207.     case COM_MANAGE_NAVIGATION:
  208.       m_eCOMState = ManageNavigation() ? COM_MANAGE_CONTROL : COM_TROUBLESHOOT;
  209.       break;
  210.  
  211.     case COM_MANAGE_CONTROL:
  212.       m_eCOMState = ManageControl() ? COM_MANAGE_SERVOS : COM_TROUBLESHOOT;
  213.       break;
  214.  
  215.     case COM_MANAGE_SERVOS:
  216.       m_eCOMState = ManageServos() ? COM_ANSWER_UART : COM_TROUBLESHOOT;
  217.       break;
  218.    
  219.     case COM_ANSWER_UART:
  220.       m_eCOMState = AnswerUART() ? COM_VERIFY_FLAGS : COM_TROUBLESHOOT;
  221.       break;
  222.    
  223.     case COM_VERIFY_FLAGS:
  224.       m_eCOMState = COM_EVALUATE_STATUS;
  225.       VerifyFlags();
  226.       break;
  227.    
  228.     case COM_EVALUATE_STATUS:
  229.       m_eCOMState = EvaluateStatus() ? COM_BROADCAST_TELEMETRY : COM_TROUBLESHOOT;
  230.       break;
  231.    
  232.     case COM_BROADCAST_TELEMETRY:
  233.       m_eCOMState = BroadcastTelemetry() ? COM_MANAGE_HEARTBEAT : COM_TROUBLESHOOT;    
  234.       break;
  235.    
  236.     case COM_MANAGE_HEARTBEAT:    
  237.       m_eCOMState = ManageHeartbeat() ? COM_INIT : COM_TROUBLESHOOT;
  238.       break;
  239.  
  240.     default: m_eCOMState = COM_INIT;
  241.   }
  242. }
  243.  
  244. bool COM::Init() {
  245.   m_bRestart = false;
  246.   return true;
  247. }
  248.  
  249. bool COM::BuildModuleCapacityMatrix() {
  250.   for (volatile uint8_t nSlot = 1; nSlot <= NUM_SLOTS; nSlot++) {
  251.     m_anCapacitiesPerSlot[nSlot] = ReadLongViaSPI(B00_MODULE_CAPACITIES, nSlot);
  252.  
  253.     for (uint8_t nCapacityBit = 0; nCapacityBit < TOTAL_CAPACITIES; nCapacityBit++) {
  254.       if (bit_is_set(m_anCapacitiesPerSlot, nCapacityBit)) {
  255.         sbi(m_anSlotListPerCapacity[nCapacityBit], nSlot);
  256.       }
  257.     }
  258.   }
  259.   m_bIdentifyModulesAgain = false;
  260.   return true;
  261. }
  262.  
  263. bool COM::ManageSensors() {
  264.   for (uint8_t nSlot = 1; nSlot < NUM_SLOTS; nSlot++) {
  265.     ReadAndFuse(BIT09_ACCELEROMETER, B09_ACCEL_X_AVG, B09_ACCEL_X_STD_DEV, nSlot);
  266.     ReadAndFuse(BIT09_ACCELEROMETER, B09_ACCEL_Y_AVG, B09_ACCEL_Y_STD_DEV, nSlot);
  267.     ReadAndFuse(BIT09_ACCELEROMETER, B09_ACCEL_Z_AVG, B09_ACCEL_Z_STD_DEV, nSlot);
  268.  
  269.     ReadAndFuse(BIT10_GYROSCOPE, B10_GYRO_X_AVG, B10_GYRO_X_STD_DEV, nSlot);
  270.     ReadAndFuse(BIT10_GYROSCOPE, B10_GYRO_Y_AVG, B10_GYRO_Y_STD_DEV, nSlot);
  271.     ReadAndFuse(BIT10_GYROSCOPE, B10_GYRO_Z_AVG, B10_GYRO_Z_STD_DEV, nSlot);
  272.  
  273.     ReadAndFuse(BIT11_MAGNETOMETER, B11_MAG_X_AVG, B11_MAG_X_STD_DEV, nSlot);
  274.     ReadAndFuse(BIT11_MAGNETOMETER, B11_MAG_X_AVG, B11_MAG_X_STD_DEV, nSlot);
  275.     ReadAndFuse(BIT11_MAGNETOMETER, B11_MAG_X_AVG, B11_MAG_X_STD_DEV, nSlot);
  276.  
  277.     ReadAndFuse(BIT12_BAROMETER, B12_BARO_P_AVG, B12_BARO_P_STD_DEV, nSlot);
  278.     ReadAndFuse(BIT12_BAROMETER, B12_BARO_T_AVG, B12_BARO_T_STD_DEV, nSlot);
  279.     ReadAndFuse(BIT12_BAROMETER, B12_BARO_ALT_AVG, B12_BARO_ALT_STD_DEV, nSlot);
  280.   }
  281.   return true;
  282. }
  283.  
  284. bool COM::ManageGPSs() {
  285.   // m_anDataArray[COM_GPS_LATITUDE]     = ReadViaSPI(GPSM_GET_GPS_LATITUDE, nSlot);
  286.   // m_anDataArray[COM_GPS_LONGITUDE]    = ReadViaSPI(GPSM_GET_GPS_LONGITUDE, nSlot);
  287.   // m_anDataArray[COM_GPS_ALTITUDE]     = ReadViaSPI(GPSM_GET_GPS_ALTITUDE, nSlot);
  288.  
  289.   return true;
  290. }
  291.  
  292. bool COM::ManageKalman() {
  293.   return true;
  294. }
  295.  
  296. bool COM::ManageGuidance() {
  297.   return true;
  298. }
  299.  
  300. bool COM::ManageNavigation() {
  301.   return true;
  302. }
  303.  
  304. bool COM::ManageControl() {
  305.   return true;
  306. }
  307.  
  308. bool COM::ManageServos() {
  309.   return true;
  310. }
  311.  
  312. bool COM::ManagePower() {
  313.   return true;
  314. }
  315.  
  316. bool COM::AnswerUART() {
  317.  
  318.   if (m_bUARTMessageReady) {
  319.  
  320.     m_bUARTMessageBeingProcessed = true;
  321.  
  322.     char achAnswer[UART_TRANSFER_BUFFER_SIZE];
  323.  
  324.     switch (m_achReceivedUARTMessage[0]) {
  325.         case 's': m_cUART.SendCharArray(FormatSlotOccupation(achAnswer)); break;
  326.        
  327.         case 'r': m_bRestart = true; break;
  328.  
  329.         case 'i': m_bIdentifyModulesAgain = true; break;
  330.  
  331.         case 'f': m_cUART.SendCharArray(FormatFreeRAM(achAnswer)); break;
  332.  
  333.         case 'C': if (m_achReceivedUARTMessage[1] == '0') {
  334.                     switch (m_achReceivedUARTMessage[2]) {
  335.                       case '0': ; break; // C00 Ask pilot to warm up engines.
  336.                     }
  337.                   }
  338.  
  339.         default: Echo(m_achReceivedUARTMessage);
  340.     }
  341.  
  342.     m_bUARTMessageReady = false;
  343.     m_bUARTMessageBeingProcessed = false;
  344.   }
  345.  
  346.   return true;
  347. }
  348.  
  349. bool COM::VerifyFlags() {
  350.  
  351.   if (m_bRestart) {
  352.     m_eCOMState = COM_INIT;
  353.   }
  354.   if (m_bIdentifyModulesAgain) {
  355.     m_eCOMState = COM_BUILD_MODULE_CAPACITY_MATRIX;
  356.   }
  357.  
  358.   return true;
  359. }
  360.  
  361. bool COM::EvaluateStatus() {
  362.  
  363.   return true;
  364. }
  365.  
  366. bool COM::BroadcastTelemetry() {
  367.  
  368.   if (m_nCycleCount > 30000) {
  369.  
  370.     char achNumber[NUMBER_BUFFER_SIZE];
  371.  
  372.     m_nCycleCount = 0;
  373.  
  374.     uint8_t nIndex;
  375.     uConvertArrayToFloat uUnpacker;
  376.  
  377.     for (nIndex = 0; nIndex < MODULE_SELECT_TOTAL_OPTIONS; nIndex++) {
  378.  
  379.       memset(achNumber, 0, NUMBER_BUFFER_SIZE);
  380.       itoa(nIndex, achNumber, 10);
  381.       m_cUART.SendCharArrayNoEOL(achNumber);
  382.  
  383.       uUnpacker.f = m_afDataArray[nIndex];
  384.       m_cUART.Write(uUnpacker.b[0]);
  385.       m_cUART.Write(uUnpacker.b[1]);
  386.       m_cUART.Write(uUnpacker.b[2]);
  387.       m_cUART.Write(uUnpacker.b[3]);
  388.       //m_cUART.Write('.');
  389.     }
  390.     m_cUART.Write('\n');
  391.   }
  392.   return true;
  393. }
  394.  
  395. bool COM::ManageHeartbeat() {
  396.  
  397.   return true;
  398. }
  399.  
  400. void COM::TroubleShoot() {
  401.  
  402. }
  403.  
  404. void COM::ReadAndFuse(uint8_t nCapacityBit, uint8_t nValueSelectOption, uint8_t nSDSelectOption, uint8_t nSlot) {
  405.  
  406.   float fValue, fStdDev; // FIXME!! Convert all fusing to float!
  407.   bool bFirst = true;
  408.  
  409.   if (bit_is_set(m_anSlotListPerCapacity[nCapacityBit], nSlot)) {
  410.  
  411.     fValue = ReadFloatViaSPI(nValueSelectOption, nSlot);
  412.     fStdDev = ReadFloatViaSPI(nSDSelectOption, nSlot);
  413.  
  414.     if (bFirst) {
  415.       bFirst = false;
  416.       m_afDataArray[nValueSelectOption] = fValue;
  417.       m_afDataArray[nSDSelectOption] = fStdDev;
  418.     }
  419.     else {
  420.       float fFusedSigma;
  421.       m_afDataArray[nValueSelectOption] = FuseValues(m_afDataArray[nValueSelectOption],
  422.                                                      m_afDataArray[nSDSelectOption],
  423.                                                      fValue,
  424.                                                      fStdDev,
  425.                                                      fFusedSigma);
  426.       m_afDataArray[nSDSelectOption] = fFusedSigma;
  427.     }
  428.   }
  429. }
  430.  
  431. float COM::FuseValues(float fZ1, float fSigma1, float fZ2, float fSigma2, float &fFusedSigma) {
  432.   float fVariance1 = pow(fSigma1, 2);
  433.   float fVariance2 = pow(fSigma2, 2);
  434.  
  435.   float fSumVariances = fVariance1 + fVariance2;
  436.  
  437.   float fFusedVariance = (fVariance1 * fVariance2) / (fVariance1 + fVariance2);
  438.  
  439.   fFusedSigma = sqrt(fFusedVariance);
  440.  
  441.   return ((fVariance2 / fSumVariances) * fZ1 + (fVariance1 / fSumVariances) * fZ2);
  442. }
  443.  
  444. void COM::SetupSlotsSS() {
  445.   // Disable Slave Selects from all slots
  446.   GPIO1_IS_OUTPUT; GPIO1_HIGH; // SS1
  447.   GPIO2_IS_OUTPUT; GPIO2_HIGH; // SS2
  448.   GPIO3_IS_OUTPUT; GPIO3_HIGH; // SS3
  449.   GPIO4_IS_OUTPUT; GPIO4_HIGH; // SS4
  450.   GPIO5_IS_OUTPUT; GPIO5_HIGH; // SS5
  451.   GPIO6_IS_OUTPUT; GPIO6_HIGH; // SS6
  452.   GPIO7_IS_OUTPUT; //GPIO7_HIGH; // SS8  // FIXME! GPIO7 is also SS7 the LED pin.
  453.   GPIO8_IS_OUTPUT; GPIO8_HIGH; // SS7
  454. }
  455.  
  456. char * COM::FormatFreeRAM(char * achBuffer) {
  457.   char achNumber[NUMBER_BUFFER_SIZE];
  458.  
  459.   memset(achBuffer, 0, UART_TRANSFER_BUFFER_SIZE);
  460.  
  461.   memset(achNumber, 0, NUMBER_BUFFER_SIZE);
  462.   itoa(FreeRAM(), achNumber, 10);
  463.   strncat(achBuffer, achNumber, UART_TRANSFER_BUFFER_SIZE);
  464.  
  465.   return achBuffer;
  466. }
  467.  
  468. void COM::ConvertAndConcatenate(DataType Value, char * achBuffer) {
  469.   char achNumber[NUMBER_BUFFER_SIZE];
  470.   ltoa(Value, achNumber, 10);
  471.   strncat(achBuffer, achNumber, UART_TRANSFER_BUFFER_SIZE);
  472. }
  473.  
  474. char * COM::FormatSlotOccupation(char * achBuffer) {
  475.   char achNumber[NUMBER_BUFFER_SIZE];
  476.   char chSeparator[3] = {':', ' ', 0};
  477.   char chSpace[2] = {' ', 0};
  478.  
  479.   memset(achBuffer, 0, UART_TRANSFER_BUFFER_SIZE);
  480.  
  481.   for (uint8_t nSlot = 1; nSlot <= NUM_SLOTS; nSlot ++) {
  482.     memset(achNumber, 0, NUMBER_BUFFER_SIZE);
  483.     itoa(nSlot, achNumber, 10);
  484.     strncat(achBuffer, achNumber, UART_TRANSFER_BUFFER_SIZE);
  485.  
  486.     strncat(achBuffer, chSeparator, UART_TRANSFER_BUFFER_SIZE);
  487.  
  488.     memset(achNumber, 0, NUMBER_BUFFER_SIZE);
  489.     ltoa(m_anCapacitiesPerSlot[nSlot], achNumber, 10);
  490.     strncat(achBuffer, achNumber, UART_TRANSFER_BUFFER_SIZE);
  491.  
  492.     strncat(achBuffer, chSpace, UART_TRANSFER_BUFFER_SIZE);
  493.   }
  494.  
  495.   return achBuffer;
  496. }
  497.  
  498. void COM::FormatLongAndSend(int32_t nNumber) {
  499.   char achNumber[15];
  500.   char chComma[2] = {',', 0};
  501.  
  502.   ltoa(nNumber, achNumber, 10);
  503.   strncat( achNumber, chComma, sizeof(achNumber));
  504.   m_cUART.SendCharArrayNoEOL(achNumber);
  505. }
  506.  
  507. void COM::SendNextByteWrapper() {
  508.   m_cUART.SendNextByte();
  509. }
  510.  
  511. void COM::ReceiveNextByteWrapper() {
  512.   m_cUART.ReceiveNextByte();
  513.   if (m_cUART.ReceptionIsComplete()) {
  514.      m_bUARTMessageReady = true;
  515.     memset(m_achReceivedUARTMessage, 0, UART_TRANSFER_BUFFER_SIZE);
  516.     m_cUART.GetIncommingMessage(m_achReceivedUARTMessage);
  517.   }
  518. }
  519.  
  520. void COM::Echo(char * achReceived) {
  521.  
  522.   m_cUART.Write(achReceived[0]);
  523.   m_cUART.Write(achReceived[1]);
  524.   m_cUART.Write(achReceived[2]);
  525.   m_cUART.Write(achReceived[3]);
  526.   m_cUART.Write(achReceived[4]);
  527.   m_cUART.Write(achReceived[5]);
  528.   m_cUART.Write(achReceived[6]);
  529.   m_cUART.Write(achReceived[7]);
  530.   m_cUART.Write(achReceived[8]);
  531.   m_cUART.Write(achReceived[9]);
  532.   m_cUART.Write('\n');
  533.  
  534.   m_cUART.SendCharArray(achReceived);
  535. }
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!