API tools faq
paste
Advertisement
enhering

Untitled

enhering
Sep 17th, 2017
90
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
SQL 19.89 KB | None | 0 0
raw download clone embed print report
  1. #include "COM.h"
  2.  
  3. ///
  4. /// YAUVeC
  5. /// Brief description. Detailed stuff.
  6. ///
  7.  
  8. COM::COM() {
  9.   m_nError = 0;
  10.   m_nCycleCount = 0;
  11.  
  12.   m_eSystemStatus = STATUS_NORMAL;
  13.  
  14.   m_bUARTMessageReady = FALSE;
  15.  
  16.   m_bSendSPIData = FALSE;
  17.  
  18.   m_cBlink.SetLEDGPIO(7);
  19.  
  20.   m_eCOMState = COM_INIT;
  21.  
  22.   m_bRestart = FALSE;
  23.   m_bIdentifyModulesAgain = FALSE;
  24.   m_bUARTMessageBeingProcessed = TRUE;
  25.  
  26.   m_bServoSetup = FALSE;
  27.   m_bServoPreSetup = TRUE;
  28. }
  29.  
  30. COM::~COM() {
  31. }
  32.  
  33. void COM::Run() {
  34.  
  35.   CustomSetup();
  36.  
  37.   while(1) {
  38.     m_cBlink.ManageLED();
  39.  
  40.     CustomLoop();  
  41.   }
  42. }
  43.  
  44. void COM::CustomSetup() {
  45.   m_cSPIProtocol.RunAsMaster();
  46.   m_cSPIProtocol.InitSPI();
  47.  
  48.   m_cUART.Init();
  49.  
  50.   m_cBlink.SetLEDGPIO(7);
  51.   m_cBlink.SetLEDOnCount(10);
  52.   m_cBlink.SetLEDOffCount(100);
  53.  
  54.   m_cBlink.BlinkLED();
  55.  
  56.   SetupSlotsSS();
  57. }
  58.  
  59. void COM::CustomLoop() {
  60.  
  61.   m_nCycleCount++;
  62.  
  63.   switch (m_eCOMState) {
  64.     CASE COM_INIT:                
  65.       m_eCOMState = Init() ? COM_BUILD_MODULE_CAPACITY_MATRIX : COM_TROUBLESHOOT;
  66.       break;
  67.    
  68.     CASE COM_BUILD_MODULE_CAPACITY_MATRIX:
  69.       m_eCOMState = BuildModuleCapacityMatrix() ? COM_MANAGE_SENSORS : COM_TROUBLESHOOT;      
  70.       break;
  71.  
  72.     CASE COM_MANAGE_POWER:
  73.       m_eCOMState = ManagePower() ? COM_MANAGE_SENSORS : COM_ANSWER_UART;
  74.       break;
  75.    
  76.     CASE COM_MANAGE_SENSORS:
  77.       m_eCOMState = ManageSensors() ? COM_MANAGE_GPSS : COM_TROUBLESHOOT;      
  78.       break;
  79.  
  80.     CASE COM_MANAGE_GPSS:
  81.       m_eCOMState = ManageGPSs() ? COM_MANAGE_KALMAN : COM_TROUBLESHOOT;
  82.       break;
  83.  
  84.     CASE COM_MANAGE_KALMAN:
  85.       m_eCOMState = ManageKalman() ? COM_MANAGE_GUIDANCE : COM_TROUBLESHOOT;
  86.       break;
  87.  
  88.     CASE COM_MANAGE_GUIDANCE:
  89.       m_eCOMState = ManageGuidance() ? COM_MANAGE_NAVIGATION : COM_TROUBLESHOOT;
  90.       break;
  91.  
  92.     CASE COM_MANAGE_NAVIGATION:
  93.       m_eCOMState = ManageNavigation() ? COM_MANAGE_CONTROL : COM_TROUBLESHOOT;
  94.       break;
  95.  
  96.     CASE COM_MANAGE_CONTROL:
  97.       m_eCOMState = ManageControl() ? COM_MANAGE_SERVOS : COM_TROUBLESHOOT;
  98.       break;
  99.  
  100.     CASE COM_MANAGE_SERVOS:
  101.       m_eCOMState = ManageServos() ? COM_ANSWER_UART : COM_TROUBLESHOOT;
  102.       break;
  103.    
  104.     CASE COM_ANSWER_UART:
  105.       m_eCOMState = AnswerUART() ? COM_VERIFY_FLAGS : COM_TROUBLESHOOT;
  106.       break;
  107.    
  108.     CASE COM_VERIFY_FLAGS:
  109.       m_eCOMState = COM_EVALUATE_STATUS;
  110.       VerifyFlags();
  111.       break;
  112.    
  113.     CASE COM_EVALUATE_STATUS:
  114.       m_eCOMState = EvaluateStatus() ? COM_BROADCAST_TELEMETRY : COM_TROUBLESHOOT;
  115.       break;
  116.    
  117.     CASE COM_BROADCAST_TELEMETRY:
  118.       m_eCOMState = BroadcastTelemetry() ? COM_MANAGE_HEARTBEAT : COM_TROUBLESHOOT;    
  119.       break;
  120.    
  121.     CASE COM_MANAGE_HEARTBEAT:    
  122.       m_eCOMState = ManageHeartbeat() ? COM_MANAGE_POWER : COM_TROUBLESHOOT;
  123.       break;
  124.  
  125.     DEFAULT: m_eCOMState = COM_INIT;
  126.   }
  127. }
  128.  
  129. bool COM::Init() {
  130.   m_bRestart = FALSE;
  131.   RETURN TRUE;
  132. }
  133.  
  134. bool COM::BuildModuleCapacityMatrix() {
  135.   FOR (uint8_t nSlot = 1; nSlot <= NUM_SLOTS; nSlot++) {
  136.     m_anCapacitiesPerSlot[nSlot] = m_cSPIProtocol.ReadLongViaSPI(B00_MODULE_CAPACITIES, nSlot);
  137.   }
  138.   FOR (uint32_t nSlot = 1; nSlot <= NUM_SLOTS; nSlot++) {  // nSlot must be uint32 too FOR the code TO WORK
  139.     FOR (uint32_t nCapacityBit = 0; nCapacityBit < TOTAL_CAPACITIES; nCapacityBit++) {
  140.       IF (m_anCapacitiesPerSlot[nSlot] & (1UL << nCapacityBit)) {
  141.         m_anSlotListPerCapacity[nCapacityBit] |= (1UL << nSlot);
  142.       }
  143.     }
  144.   }
  145.   m_bIdentifyModulesAgain = FALSE;
  146.   RETURN TRUE;
  147. }
  148.  
  149. bool COM::ManageSensors() {
  150.   FOR (uint8_t nSlot = 1; nSlot < NUM_SLOTS; nSlot++) {
  151.     ReadAndFuse(BIT09_ACCELEROMETER, B09_ACCEL_X_AVG, B09_ACCEL_X_STD_DEV, nSlot);
  152.     ReadAndFuse(BIT09_ACCELEROMETER, B09_ACCEL_Y_AVG, B09_ACCEL_Y_STD_DEV, nSlot);
  153.     ReadAndFuse(BIT09_ACCELEROMETER, B09_ACCEL_Z_AVG, B09_ACCEL_Z_STD_DEV, nSlot);
  154.  
  155.     ReadAndFuse(BIT10_GYROSCOPE, B10_GYRO_X_AVG, B10_GYRO_X_STD_DEV, nSlot);
  156.     ReadAndFuse(BIT10_GYROSCOPE, B10_GYRO_Y_AVG, B10_GYRO_Y_STD_DEV, nSlot);
  157.     ReadAndFuse(BIT10_GYROSCOPE, B10_GYRO_Z_AVG, B10_GYRO_Z_STD_DEV, nSlot);
  158.  
  159.     ReadAndFuse(BIT11_MAGNETOMETER, B11_MAG_X_AVG, B11_MAG_X_STD_DEV, nSlot);
  160.     ReadAndFuse(BIT11_MAGNETOMETER, B11_MAG_Y_AVG, B11_MAG_Y_STD_DEV, nSlot);
  161.     ReadAndFuse(BIT11_MAGNETOMETER, B11_MAG_Z_AVG, B11_MAG_Z_STD_DEV, nSlot);
  162.  
  163.     ReadAndFuse(BIT12_BAROMETER, B12_BARO_P_AVG, B12_BARO_P_STD_DEV, nSlot);
  164.     ReadAndFuse(BIT12_BAROMETER, B12_BARO_T_AVG, B12_BARO_T_STD_DEV, nSlot);
  165.     ReadAndFuse(BIT12_BAROMETER, B12_BARO_ALT_AVG, B12_BARO_ALT_STD_DEV, nSlot);
  166.   }
  167.   RETURN TRUE;
  168. }
  169.  
  170. bool COM::ManageGPSs() {
  171.   // m_anDataArray[COM_GPS_LATITUDE]     = ReadViaSPI(GPSM_GET_GPS_LATITUDE, nSlot);
  172.   // m_anDataArray[COM_GPS_LONGITUDE]    = ReadViaSPI(GPSM_GET_GPS_LONGITUDE, nSlot);
  173.   // m_anDataArray[COM_GPS_ALTITUDE]     = ReadViaSPI(GPSM_GET_GPS_ALTITUDE, nSlot);
  174.  
  175.   RETURN TRUE;
  176. }
  177.  
  178. bool COM::ManageKalman() {
  179.   RETURN TRUE;
  180. }
  181.  
  182. bool COM::ManageGuidance() {
  183.   RETURN TRUE;
  184. }
  185.  
  186. bool COM::ManageNavigation() {
  187.   FOR (volatile uint8_t nSlot = 1; nSlot <= NUM_SLOTS; nSlot++) {
  188.     IF (m_anCapacitiesPerSlot[nSlot] & _BV(BIT03_NAVIGATION)) {
  189.       // SEND sensor DATA
  190.       m_cSPIProtocol.SendLongViaSPI(B09_ACCEL_X_AVG, nSlot, m_anDataArray[B09_ACCEL_X_AVG]);
  191.       m_cSPIProtocol.SendLongViaSPI(B09_ACCEL_Y_AVG, nSlot, m_anDataArray[B09_ACCEL_Y_AVG]);
  192.       m_cSPIProtocol.SendLongViaSPI(B09_ACCEL_Z_AVG, nSlot, m_anDataArray[B09_ACCEL_Z_AVG]);
  193.       m_cSPIProtocol.SendLongViaSPI(B09_ACCEL_X_AVG, nSlot, m_anDataArray[B09_ACCEL_X_STD_DEV]);
  194.       m_cSPIProtocol.SendLongViaSPI(B09_ACCEL_Y_AVG, nSlot, m_anDataArray[B09_ACCEL_Y_STD_DEV]);
  195.       m_cSPIProtocol.SendLongViaSPI(B09_ACCEL_Z_AVG, nSlot, m_anDataArray[B09_ACCEL_Z_STD_DEV]);
  196.  
  197.       m_cSPIProtocol.SendLongViaSPI(B10_GYRO_X_AVG, nSlot, m_anDataArray[B10_GYRO_X_AVG]);
  198.       m_cSPIProtocol.SendLongViaSPI(B10_GYRO_Y_AVG, nSlot, m_anDataArray[B10_GYRO_Y_AVG]);
  199.       m_cSPIProtocol.SendLongViaSPI(B10_GYRO_Z_AVG, nSlot, m_anDataArray[B10_GYRO_Z_AVG]);
  200.       m_cSPIProtocol.SendLongViaSPI(B10_GYRO_X_AVG, nSlot, m_anDataArray[B10_GYRO_X_STD_DEV]);
  201.       m_cSPIProtocol.SendLongViaSPI(B10_GYRO_Y_AVG, nSlot, m_anDataArray[B10_GYRO_Y_STD_DEV]);
  202.       m_cSPIProtocol.SendLongViaSPI(B10_GYRO_Z_AVG, nSlot, m_anDataArray[B10_GYRO_Z_STD_DEV]);
  203.  
  204.       m_cSPIProtocol.SendLongViaSPI(B11_MAG_X_AVG, nSlot, m_anDataArray[B11_MAG_X_AVG]);
  205.       m_cSPIProtocol.SendLongViaSPI(B11_MAG_Y_AVG, nSlot, m_anDataArray[B11_MAG_Y_AVG]);
  206.       m_cSPIProtocol.SendLongViaSPI(B11_MAG_Z_AVG, nSlot, m_anDataArray[B11_MAG_Z_AVG]);
  207.       m_cSPIProtocol.SendLongViaSPI(B11_MAG_X_AVG, nSlot, m_anDataArray[B11_MAG_X_STD_DEV]);
  208.       m_cSPIProtocol.SendLongViaSPI(B11_MAG_Y_AVG, nSlot, m_anDataArray[B11_MAG_Y_STD_DEV]);
  209.       m_cSPIProtocol.SendLongViaSPI(B11_MAG_Z_AVG, nSlot, m_anDataArray[B11_MAG_Z_STD_DEV]);
  210.  
  211.       m_cSPIProtocol.SendLongViaSPI(B12_BARO_P_AVG, nSlot, m_anDataArray[B12_BARO_P_AVG]);
  212.       m_cSPIProtocol.SendLongViaSPI(B12_BARO_T_AVG, nSlot, m_anDataArray[B12_BARO_T_AVG]);
  213.       m_cSPIProtocol.SendLongViaSPI(B12_BARO_ALT_AVG, nSlot, m_anDataArray[B12_BARO_ALT_AVG]);
  214.       m_cSPIProtocol.SendLongViaSPI(B12_BARO_P_AVG, nSlot, m_anDataArray[B12_BARO_P_STD_DEV]);
  215.       m_cSPIProtocol.SendLongViaSPI(B12_BARO_T_AVG, nSlot, m_anDataArray[B12_BARO_T_STD_DEV]);
  216.       m_cSPIProtocol.SendLongViaSPI(B12_BARO_ALT_AVG, nSlot, m_anDataArray[B12_BARO_ALT_STD_DEV]);
  217.  
  218.       // READ navigation DATA
  219.  
  220.       // FIXME: THIS DATA SHOULD BE FUSED
  221.       // FIXME: THIS DATA SHOULD CONTAIN ERROR INFORMATION
  222.  
  223.       // m_anDataArray[B03_PHI]       = m_cSPIProtocol.ReadLongViaSPI(B03_PHI, nSlot);
  224.       // m_anDataArray[B03_PHI_DOT]   = m_cSPIProtocol.ReadLongViaSPI(B03_PHI_DOT, nSlot);
  225.       // m_anDataArray[B03_PSI]       = m_cSPIProtocol.ReadLongViaSPI(B03_PSI, nSlot);
  226.       // m_anDataArray[B03_PSI_DOT]   = m_cSPIProtocol.ReadLongViaSPI(B03_PSI_DOT, nSlot);
  227.       // m_anDataArray[B03_THETA]     = m_cSPIProtocol.ReadLongViaSPI(B03_THETA, nSlot);
  228.       // m_anDataArray[B03_THETA_DOT] = m_cSPIProtocol.ReadLongViaSPI(B03_THETA_DOT, nSlot);
  229.     }
  230.   }
  231.   RETURN TRUE;
  232. }
  233.  
  234. bool COM::ManageControl() {
  235.   IF (m_bServoPreSetup) {
  236.     RETURN TRUE;
  237.     // Query servo module FOR NUMBER OF servos etc.
  238.   }
  239.  
  240.   FOR (volatile uint8_t nSlot = 1; nSlot <= NUM_SLOTS; nSlot++) {
  241.     IF (m_anCapacitiesPerSlot[nSlot] & _BV(BIT04_CONTROL)) {
  242.      
  243.       // m_cSPIProtocol.SendLongViaSPI(B03_PHI,       nSlot, m_anDataArray[B03_PHI]);
  244.       // m_cSPIProtocol.SendLongViaSPI(B03_PHI_DOT,   nSlot, m_anDataArray[B03_PHI_DOT]);
  245.       // m_cSPIProtocol.SendLongViaSPI(B03_PSI,       nSlot, m_anDataArray[B03_PSI]);
  246.       // m_cSPIProtocol.SendLongViaSPI(B03_PSI_DOT,   nSlot, m_anDataArray[B03_PSI_DOT]);
  247.       // m_cSPIProtocol.SendLongViaSPI(B03_THETA,     nSlot, m_anDataArray[B03_THETA]);
  248.       // m_cSPIProtocol.SendLongViaSPI(B03_THETA_DOT, nSlot, m_anDataArray[B03_THETA_DOT]);
  249.  
  250.       IF (m_bServoSetup) {
  251.         m_cSPIProtocol.SendByteViaSPI(B17_NUM_SERVO_CHANNELS, nSlot, m_nNumServoChannels);
  252.  
  253.         FOR (uint8_t nServoNum = 0; nServoNum < m_nNumServoChannels; nServoNum++) {
  254.  
  255.           m_cSPIProtocol.SendByteViaSPI(B17_SERVO_SELECT, nSlot, nServoNum);
  256.  
  257.           m_anServoMin[nServoNum]    = m_cSPIProtocol.ReadIntViaSPI(B17_SERVO_MIN, nSlot);
  258.           m_anServoMax[nServoNum]    = m_cSPIProtocol.ReadIntViaSPI(B17_SERVO_MAX, nSlot);
  259.           m_anServoZero[nServoNum]   = m_cSPIProtocol.ReadIntViaSPI(B17_SERVO_ZERO, nSlot);
  260.           m_abServoEnable[nServoNum] = (bool) m_cSPIProtocol.ReadByteViaSPI(B17_SERVO_ENABLE, nSlot);
  261.           m_anServoPos[nServoNum]    = 0;
  262.         }
  263.         RETURN TRUE;
  264.       }
  265.       // NORMAL OPERATION
  266.       FOR (uint8_t nServoNum = 0; nServoNum < m_nNumServoChannels; nServoNum++) {
  267.         IF (m_abServoEnable[nServoNum]) {
  268.           m_anServoPos[nServoNum] = m_cSPIProtocol.ReadIntViaSPI(B17_SERVO_POS, nSlot);
  269.         }
  270.       }
  271.     }
  272.   }
  273.  
  274.   RETURN TRUE;
  275. }
  276.  
  277. bool COM::ManageServos() {
  278.  
  279.   FOR (volatile uint8_t nSlot = 1; nSlot <= NUM_SLOTS; nSlot++) {
  280.     IF (m_anCapacitiesPerSlot[nSlot] & (1UL << BIT17_SERVO_CONTROLER)) {
  281.       IF (m_bServoPreSetup) {
  282.         m_nNumServoChannels = m_cSPIProtocol.ReadByteViaSPI(B17_NUM_SERVO_CHANNELS, nSlot);
  283.         m_bServoPreSetup = FALSE;
  284.         m_bServoSetup = TRUE;
  285.         RETURN TRUE;
  286.       }
  287.  
  288.       IF (m_bServoSetup) {
  289.         FOR (uint8_t nServoNum = 0; nServoNum < m_nNumServoChannels; nServoNum++) {
  290.  
  291.           m_cSPIProtocol.SendByteViaSPI(B17_SERVO_SELECT, nSlot, nServoNum);
  292.  
  293.           IF (m_abServoEnable[nServoNum]) {
  294.             m_cSPIProtocol.SendIntViaSPI(B17_SERVO_MIN, nSlot, m_anServoMin[nServoNum]);    
  295.             m_cSPIProtocol.SendIntViaSPI(B17_SERVO_MAX, nSlot, m_anServoMax[nServoNum]);    
  296.             m_cSPIProtocol.SendIntViaSPI(B17_SERVO_ZERO, nSlot, m_anServoZero[nServoNum]);  
  297.             m_cSPIProtocol.SendIntViaSPI(B17_SERVO_POS, nSlot, m_anServoPos[nServoNum]);
  298.           }
  299.           m_cSPIProtocol.SendByteViaSPI(B17_SERVO_ENABLE, nSlot, m_abServoEnable[nServoNum]);
  300.         }
  301.  
  302.         m_bServoSetup = FALSE;
  303.         RETURN TRUE;
  304.       }
  305.  
  306.       FOR (uint8_t nServoNum = 0; nServoNum < m_nNumServoChannels; nServoNum++) {
  307.         IF (m_abServoEnable[nServoNum]) {
  308.           m_cSPIProtocol.SendByteViaSPI(B17_SERVO_SELECT, nSlot, nServoNum);
  309.           m_cSPIProtocol.SendIntViaSPI(B17_SERVO_POS, nSlot, m_anServoPos[nServoNum]);
  310.         }
  311.       }
  312.     }
  313.   }
  314.  
  315.   RETURN TRUE;
  316. }
  317.  
  318. bool COM::ManagePower() {
  319.  
  320.   RETURN TRUE;
  321. }
  322.  
  323. bool COM::AnswerUART() {
  324.  
  325.   IF (m_bUARTMessageReady) {
  326.  
  327.     m_bUARTMessageBeingProcessed = TRUE;
  328.  
  329.     CHAR achAnswer[UART_TRANSFER_BUFFER_SIZE];
  330.  
  331.     switch (m_achReceivedUARTMessage[0]) {
  332.         CASE 's': m_cUART.SendCharArray(FormatSlotOccupation(achAnswer)); break;
  333.        
  334.         CASE 'r': m_bRestart = TRUE; break;
  335.  
  336.         CASE 'i': m_bIdentifyModulesAgain = TRUE; break;
  337.  
  338.         CASE 'f': m_cUART.SendCharArray(FormatFreeRAM(achAnswer)); break;
  339.  
  340.         CASE 'c': FormatCapacitierPerSlotAndSend(); break;
  341.  
  342.         // CASE 'D': IF (m_achReceivedUARTMessage[1] == '0') {
  343.         //             switch (m_achReceivedUARTMessage[2]) {
  344.         //               CASE '0': m_cUART.SendCharArray(Format3DData(    m_sAccelerometer,        achAnswer)); break;
  345.         //               CASE '1': m_cUART.SendCharArray(Format3DData(    m_sGyroscope,            achAnswer)); break;
  346.         //               CASE '2': m_cUART.SendCharArray(Format3DData(    m_sMagnetometer,         achAnswer)); break;
  347.         //               CASE '3': m_cUART.SendCharArray(FormatScalarData(m_sBarometerAltitude,    achAnswer)); break;
  348.         //               CASE '4': m_cUART.SendCharArray(FormatScalarData(m_sBarometerPressure,    achAnswer)); break;
  349.         //               CASE '5': m_cUART.SendCharArray(FormatScalarData(m_sBarometerTemperature, achAnswer)); break;
  350.         //               CASE '6': m_cUART.SendCharArray(FormatScalarData(m_sPsi,                  achAnswer)); break;
  351.         //               CASE '7': m_cUART.SendCharArray(FormatScalarData(m_sPsiDot,               achAnswer)); break;
  352.         //               CASE '8': m_cUART.SendCharArray(FormatScalarData(m_sPhi,                  achAnswer)); break;
  353.         //               CASE '9': m_cUART.SendCharArray(FormatScalarData(m_sPhiDot,               achAnswer)); break;
  354.         //             }
  355.         //           }
  356.  
  357.         //           IF (m_achReceivedUARTMessage[1] == '1') {
  358.         //             switch (m_achReceivedUARTMessage[2]) {
  359.         //               CASE '0': m_cUART.SendCharArray(FormatScalarData(m_sTheta,                achAnswer)); break;
  360.         //               CASE '1': m_cUART.SendCharArray(FormatScalarData(m_sThetaDot,             achAnswer)); break;
  361.         //               CASE '2': m_cUART.SendCharArray(FormatScalarData(m_sLatitude,             achAnswer)); break;
  362.         //               CASE '3': m_cUART.SendCharArray(FormatScalarData(m_sLongitude,            achAnswer)); break;
  363.         //               CASE '4': m_cUART.SendCharArray(FormatScalarData(m_sGPSAltitude,          achAnswer)); break;
  364.         //               CASE '5': break;
  365.         //               CASE '6': break;
  366.         //               CASE '7': break;
  367.         //               CASE '8': break;
  368.         //               CASE '9': break;
  369.         //             }
  370.         //           }
  371.  
  372.         //           break;
  373.  
  374.         CASE 'C': IF (m_achReceivedUARTMessage[1] == '0') {
  375.                     switch (m_achReceivedUARTMessage[2]) {
  376.                       CASE '0': ; break; // C00 Ask pilot TO warm up engines.
  377.                     }
  378.                   }
  379.  
  380.         DEFAULT: Echo(m_achReceivedUARTMessage);
  381.     }
  382.  
  383.     m_bUARTMessageReady = FALSE;
  384.     m_bUARTMessageBeingProcessed = FALSE;
  385.   }
  386.  
  387.   RETURN TRUE;
  388. }
  389.  
  390. bool COM::VerifyFlags() {
  391.  
  392.   IF (m_bRestart) {
  393.     m_eCOMState = COM_INIT;
  394.   }
  395.   IF (m_bIdentifyModulesAgain) {
  396.     m_eCOMState = COM_BUILD_MODULE_CAPACITY_MATRIX;
  397.   }
  398.  
  399.   RETURN TRUE;
  400. }
  401.  
  402. bool COM::EvaluateStatus() {
  403.  
  404.   RETURN TRUE;
  405. }
  406.  
  407. bool COM::BroadcastTelemetry() {
  408.  
  409.   IF (m_nCycleCount > 300) {
  410.  
  411.     // CHAR achNumber[NUMBER_BUFFER_SIZE];
  412.  
  413.     m_nCycleCount = 0;
  414.  
  415.     uint8_t nIndex;
  416.     uConvertArrayToInt32 uUnpacker;
  417.  
  418.     m_cUART.WRITE('#');
  419.     m_cUART.WRITE('#');
  420.     m_cUART.WRITE('#');
  421.  
  422.     FOR (nIndex = 0; nIndex < MODULE_SELECT_TOTAL_OPTIONS; nIndex++) {
  423.  
  424.       // memset(achNumber, 0, NUMBER_BUFFER_SIZE);
  425.       // itoa(nIndex, achNumber, 10);
  426.       // m_cUART.SendCharArrayNoEOL(achNumber);
  427.  
  428.       uUnpacker.i = m_anDataArray[nIndex];
  429.       m_cUART.WRITE(uUnpacker.b[0]);
  430.       m_cUART.WRITE(uUnpacker.b[1]);
  431.       m_cUART.WRITE(uUnpacker.b[2]);
  432.       m_cUART.WRITE(uUnpacker.b[3]);
  433.      
  434.       IF (nIndex != (MODULE_SELECT_TOTAL_OPTIONS - 1)) {
  435.         m_cUART.WRITE('#'); // Separator
  436.       }
  437.     }
  438.     m_cUART.WRITE('\n');
  439.   }
  440.   RETURN TRUE;
  441. }
  442.  
  443. bool COM::ManageHeartbeat() {
  444.  
  445.   RETURN TRUE;
  446. }
  447.  
  448. void COM::TroubleShoot() {
  449.  
  450. }
  451.  
  452. void COM::ReadAndFuse(uint8_t nCapacityBit, uint8_t nValueSelectOption, uint8_t nSDSelectOption, uint8_t nSlot) {
  453.  
  454.   int32_t nValue, nStdDev;
  455.   bool bFirst = TRUE;
  456.  
  457.   IF (m_anSlotListPerCapacity[nCapacityBit] & (1UL << nSlot)) {
  458.  
  459.     nValue = m_cSPIProtocol.ReadLongViaSPI(nValueSelectOption, nSlot);
  460.     nStdDev = m_cSPIProtocol.ReadLongViaSPI(nSDSelectOption, nSlot);
  461.  
  462.     IF (bFirst) {
  463.       bFirst = FALSE;
  464.       m_anDataArray[nValueSelectOption] = nValue;
  465.       m_anDataArray[nSDSelectOption] = nStdDev;
  466.     }
  467.     ELSE {
  468.       int32_t nFusedSigma;
  469.       m_anDataArray[nValueSelectOption] = FuseValues(m_anDataArray[nValueSelectOption],
  470.                                                      m_anDataArray[nSDSelectOption],
  471.                                                      nValue,
  472.                                                      nStdDev,
  473.                                                      nFusedSigma);
  474.       m_anDataArray[nSDSelectOption] = nFusedSigma;
  475.     }
  476.   }
  477. }
  478.  
  479. int32_t COM::FuseValues(int32_t nZ1, int32_t nSigma1, int32_t nZ2, int32_t nSigma2, int32_t &nFusedSigma) {
  480.   int32_t nVariance1 = pow(nSigma1, 2);
  481.   int32_t nVariance2 = pow(nSigma2, 2);
  482.  
  483.   int32_t nSumVariances = nVariance1 + nVariance2;
  484.  
  485.   int32_t nFusedVariance = (nVariance1 * nVariance2) / (nVariance1 + nVariance2);
  486.  
  487.   nFusedSigma = isqrt(nFusedVariance);
  488.  
  489.   RETURN ((nVariance2 / nSumVariances) * nZ1 + (nVariance1 / nSumVariances) * nZ2);
  490. }
  491.  
  492. void COM::SetupSlotsSS() {
  493.   // Disable Slave Selects FROM ALL slots
  494.   GPIO1_IS_OUTPUT; GPIO1_HIGH; // SS1
  495.   GPIO2_IS_OUTPUT; GPIO2_HIGH; // SS2
  496.   GPIO3_IS_OUTPUT; GPIO3_HIGH; // SS3
  497.   GPIO4_IS_OUTPUT; GPIO4_HIGH; // SS4
  498.   GPIO5_IS_OUTPUT; GPIO5_HIGH; // SS5
  499.   GPIO6_IS_OUTPUT; GPIO6_HIGH; // SS6
  500.   GPIO7_IS_OUTPUT; //GPIO7_HIGH; // SS8  // FIXME! GPIO7 IS also SS7 the LED pin.
  501.   GPIO8_IS_OUTPUT; GPIO8_HIGH; // SS7
  502. }
  503.  
  504. CHAR * COM::FormatFreeRAM(CHAR * achBuffer) {
  505.   CHAR achNumber[NUMBER_BUFFER_SIZE];
  506.  
  507.   memset(achBuffer, 0, UART_TRANSFER_BUFFER_SIZE);
  508.  
  509.   memset(achNumber, 0, NUMBER_BUFFER_SIZE);
  510.   itoa(FreeRAM(), achNumber, 10);
  511.   strncat(achBuffer, achNumber, UART_TRANSFER_BUFFER_SIZE);
  512.  
  513.   RETURN achBuffer;
  514. }
  515.  
  516. void COM::ConvertAndConcatenate(DataType VALUE, CHAR * achBuffer) {
  517.   CHAR achNumber[NUMBER_BUFFER_SIZE];
  518.   ltoa(VALUE, achNumber, 10);
  519.   strncat(achBuffer, achNumber, UART_TRANSFER_BUFFER_SIZE);
  520. }
  521.  
  522. CHAR * COM::FormatSlotOccupation(CHAR * achBuffer) {
  523.   CHAR achNumber[NUMBER_BUFFER_SIZE];
  524.   CHAR chSeparator[3] = {':', ' ', 0};
  525.   CHAR chSpace[2] = {' ', 0};
  526.  
  527.   memset(achBuffer, 0, UART_TRANSFER_BUFFER_SIZE);
  528.  
  529.   FOR (uint8_t nSlot = 1; nSlot <= NUM_SLOTS; nSlot ++) {
  530.     memset(achNumber, 0, NUMBER_BUFFER_SIZE);
  531.     itoa(nSlot, achNumber, 10);
  532.     strncat(achBuffer, achNumber, UART_TRANSFER_BUFFER_SIZE);
  533.  
  534.     strncat(achBuffer, chSeparator, UART_TRANSFER_BUFFER_SIZE);
  535.  
  536.     memset(achNumber, 0, NUMBER_BUFFER_SIZE);
  537.     ltoa(m_anCapacitiesPerSlot[nSlot], achNumber, 10);
  538.     strncat(achBuffer, achNumber, UART_TRANSFER_BUFFER_SIZE);
  539.  
  540.     strncat(achBuffer, chSpace, UART_TRANSFER_BUFFER_SIZE);
  541.   }
  542.  
  543.   RETURN achBuffer;
  544. }
  545.  
  546. void COM::FormatCapacitierPerSlotAndSend() {
  547.  
  548.   FOR (uint32_t nCapacityBit = 0; nCapacityBit < TOTAL_CAPACITIES; nCapacityBit++) {
  549.  
  550.     m_cUART.WRITE('\n');
  551.  
  552.     FormatLongAndSend(nCapacityBit);
  553.  
  554.     FOR (volatile uint32_t nSlot = 1; nSlot <= NUM_SLOTS; nSlot++) {
  555.       IF (m_anSlotListPerCapacity[nCapacityBit] & (uint32_t)(1UL << nSlot)) {
  556.         m_cUART.WRITE('X');
  557.       }
  558.       ELSE {
  559.         m_cUART.WRITE('.');
  560.       }
  561.     }
  562.   }
  563.   m_cUART.WRITE('\n');
  564. }
  565.  
  566. void COM::FormatLongAndSend(int32_t nNumber) {
  567.   CHAR achNumber[15];
  568.   // CHAR chComma[2] = {',', 0};
  569.  
  570.   ltoa(nNumber, achNumber, 10);
  571.   // strncat( achNumber, chComma, sizeof(achNumber));
  572.   m_cUART.SendCharArrayNoEOL(achNumber);
  573. }
  574.  
  575. void COM::SendNextByteWrapper() {
  576.   m_cUART.SendNextByte();
  577. }
  578.  
  579. void COM::ReceiveNextByteWrapper() {
  580.   m_cUART.ReceiveNextByte();
  581.   IF (m_cUART.ReceptionIsComplete()) {
  582.      m_bUARTMessageReady = TRUE;
  583.     memset(m_achReceivedUARTMessage, 0, UART_TRANSFER_BUFFER_SIZE);
  584.     m_cUART.GetIncommingMessage(m_achReceivedUARTMessage);
  585.   }
  586. }
  587.  
  588. void COM::Echo(CHAR * achReceived) {
  589.  
  590.   m_cUART.WRITE(achReceived[0]);
  591.   m_cUART.WRITE(achReceived[1]);
  592.   m_cUART.WRITE(achReceived[2]);
  593.   m_cUART.WRITE(achReceived[3]);
  594.   m_cUART.WRITE(achReceived[4]);
  595.   m_cUART.WRITE(achReceived[5]);
  596.   m_cUART.WRITE(achReceived[6]);
  597.   m_cUART.WRITE(achReceived[7]);
  598.   m_cUART.WRITE(achReceived[8]);
  599.   m_cUART.WRITE(achReceived[9]);
  600.   m_cUART.WRITE('\n');
  601.  
  602.   m_cUART.SendCharArray(achReceived);
  603. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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!