hormone v.1 for optimizing COT

Dependents:   TurtleBot_with_AHS

Fork of calculator by worasuchad haomachai

Revision:
1:b6345fbad095
Parent:
0:22ebaf70b60e
--- a/calculator.cpp	Mon Aug 13 11:38:52 2018 +0000
+++ b/calculator.cpp	Fri Aug 24 10:57:58 2018 +0000
@@ -19,7 +19,7 @@
 /* RETURN VALUE : SD value                                            */ 
 /* DESCRIPTION  : -                                                   */ 
 /*--------------------------------------------------------------------*/ 
-float calculator::calculateSD(const float *sdData, int size)
+float calculator::calculateSD(const float *sdData, const int size)
 {
     float sum = 0.0, mean, standardDeviation = 0.0;
     int i;
@@ -51,7 +51,7 @@
 /* RETURN VALUE : mean value                                          */ 
 /* DESCRIPTION  : -                                                   */ 
 /*--------------------------------------------------------------------*/
-float calculator::calculateMean(const float *meanData, int size)
+float calculator::calculateMean(const float *meanData, const int size)
 {
     float sum = 0.0, mean;
     int i;
@@ -77,7 +77,7 @@
 /* RETURN VALUE : size value                                          */ 
 /* DESCRIPTION  : -                                                   */ 
 /*--------------------------------------------------------------------*/
-float calculator::calcSDVectorA(const float *arrAx, const float *arrAy, const float *arrAz, int size)
+float calculator::calcSDVectorA(const float *arrAx, const float *arrAy, const float *arrAz, const int size)
 {
     float sum = 0.0, mean = 0.0;
     float standardDeviation = 0.0;
@@ -129,7 +129,7 @@
 /* RETURN VALUE : mean value                                          */ 
 /* DESCRIPTION  : -                                                   */ 
 /*--------------------------------------------------------------------*/
-float calculator::calcG4(const float *arrRoll, const float *arrPitch, int size)
+float calculator::calcG4(const float *arrRoll, const float *arrPitch, const int sizeG4)
 {
     float sum = 0.0, mean = 0.0;
     int i; 
@@ -141,7 +141,7 @@
         return NULL;
     }
     
-    pSize = (float *)malloc(size*sizeof(float));
+    pSize = (float *)malloc(sizeG4*sizeof(float));
     /* check malloc is not return NULL */
     if( pSize == NULL )
     {
@@ -150,19 +150,19 @@
     /*--------------------------------------------------------------------*/ 
     /*                  calculate size of point                           */ 
     /*--------------------------------------------------------------------*/ 
-    for(i = 0; i < size; i++)
+    for(i = 0; i < sizeG4; i++)
     {
         *(pSize + i) = sqrt( pow(arrRoll[i],2) + pow(arrPitch[i],2) );   
     }    
     /*--------------------------------------------------------------------*/ 
     /*                  calculate mean of point size                      */ 
     /*--------------------------------------------------------------------*/
-    for(i = 0; i < size ; i++)
+    for(i = 0; i < sizeG4 ; i++)
     {
         sum += *(pSize + i);
         //pc.printf("sum = %.2f \n\r",sum);    
     }    
-    mean = sum/size;
+    mean = sum/sizeG4;
     //pc.printf("mean = %.2f \n\r",mean); 
     
     free(pSize);