Diff: systemVar.cpp

Revision:

0:81b21910454e

Child:

1:b4a0d63db637

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/systemVar.cpp	Tue Jun 23 13:55:28 2015 +0000
@@ -0,0 +1,151 @@
+//////////////////////////////////////////////////////////////////////////////////////
+//                                                                                  //
+//      File        : systemVar.cpp                                                 //
+//      Version     : 0.1                                                           //
+//      Date        : 23 june 2015                                                  //
+//      Author      : Dany Brugman                                                  //
+//      Comment     : Class systemVar to manage and control system variables        //
+//                                                                                  //
+//      Changelog   :                                                               //
+//      Date:           Name:       Comment:                                        //
+//      23/06/2015      DNB         First version                                   //
+//                                                                                  //
+//////////////////////////////////////////////////////////////////////////////////////
+#include "menu.h"
+#include "systemVar.h"
+
+
+//////////////////////////////////////////////////////////////////////////////////////
+// Contructor                                                                       //
+//////////////////////////////////////////////////////////////////////////////////////
+SystemVar::SystemVar() : 
+           uiCounter(0),
+           bError(0)           
+        {};
+
+SystemVar::~SystemVar()
+{
+};
+
+
+extern Serial debug;
+//////////////////////////////////////////////////////////////////////////////////////
+// set's                                                                            //
+////////////////////////////////////////////////////////////////////////////////////// 
+void SystemVar::vSetRoll(Quaternion q1)
+{
+    float fRoll;
+    uint32_t uiValue;
+    fRoll = atan2(2*(q1.v.x*q1.v.y + q1.w*q1.v.y), q1.w*q1.w + q1.v.x*q1.v.x - q1.v.y*q1.v.y - q1.v.y*q1.v.y);
+    fRoll *= 180/3.14;
+    if(fRoll >= 0) uiRoll[0] = 1;
+    else uiRoll[0] = 0;
+    uiValue = (uint32_t) abs(fRoll);
+    uiRoll[1] = uiValue;
+}
+
+void SystemVar::vSetPitch(Quaternion q1)
+{
+    float fPitch;
+    uint32_t uiValue;
+    Vector3 euler = q1.getEulerAngles();
+    fPitch = euler.y;
+    fPitch *= 180/3.14;
+    if(fPitch >= 0) uiPitch[0] = 1;
+    else uiPitch[0] = 0;
+    uiValue = (uint32_t) abs(fPitch);
+    uiPitch[1] = uiValue;        
+}
+        
+void SystemVar::vSetPHeight(uint32_t uiValue)
+{
+    uiPHeight = uiValue;
+}
+
+void SystemVar::vSetSHeight(uint32_t uiValue)
+{
+    uiSHeight = uiValue;
+}
+
+void SystemVar::vSetPCurrent(uint32_t uiValue)
+{
+     uiPCurrent = uiValue;   
+}
+        
+void SystemVar::vSetSCurrent(uint32_t uiValue)
+{
+    uiSCurrent = uiValue; 
+}
+
+void SystemVar::vSetSpeed(uint32_t uiValue)
+{
+    uiSpeed = uiValue;
+}
+
+//////////////////////////////////////////////////////////////////////////////////////
+// get's                                                                            //
+////////////////////////////////////////////////////////////////////////////////////// 
+char* SystemVar::getValue(void) 
+{
+     static char* pValue;
+     pValue = cValue;
+     return pValue;
+}
+
+int SystemVar::iGetRollPolarity(void)
+{
+    return uiRoll[0];
+}
+
+int SystemVar::iGetPitchPolarity(void)
+{
+    return uiPitch[0];
+}
+
+//////////////////////////////////////////////////////////////////////////////////////
+// show value                                                                       //
+//////////////////////////////////////////////////////////////////////////////////////
+void SystemVar::itoa( uint32_t value, char *str)
+{   
+   int i,j;
+   char temp[4];
+   for(i=0; value > 0; i++){    
+       str[i] = value%10+'0';
+       value=value/10;
+    }
+    for(j=0;i>=0;j++,i--){
+        temp[j]=str[i-1];
+    }
+    for(i=0;i<j;i++){
+        str[i]=temp[i];
+    }
+    if(strcmp(str,"")== 0) str[0] = '0';
+       
+}
+
+//////////////////////////////////////////////////////////////////////////////////////
+// show value                                                                       //
+////////////////////////////////////////////////////////////////////////////////////// 
+void SystemVar::cShowValue(VALUE_t v)
+{
+    value = v;
+    //debug.printf("SW case %i\t", uiRoll[1]);
+    switch(value)
+    {
+        case _ROLL:             itoa(uiRoll[1], cValue); break;
+                                
+        case _PITCH:            itoa(uiPitch[1], cValue); break;
+        
+        
+        
+    } // end switch
+    
+}
+
+
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////
+// EOF                                                                              //
+//////////////////////////////////////////////////////////////////////////////////////