transformations for accel representations

paparazzi · Feb 23, 2011 · 2e19438 · 2e19438
1 parent 8737e18
commit 2e19438
Showing 1 changed file with 74 additions and 18 deletions.
diff --git a/sw/airborne/state.c b/sw/airborne/state.c
@@ -524,12 +524,29 @@ void stateCalcAccelNed_i(void) {
   if (bit_is_set(state.accel_status, ACCEL_NED_I))
     return;
 
-  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
-    if (state.ned_initialised_i) {
+  int errno = 0;
+  if (state.ned_initialised_i) {
+    if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
+      ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f);
+    }
+    else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
       ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i);
     }
-  } else {
-    //try floats....
+    else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
+      /* transform ecef_f -> ecef_i -> ned_i , set status bits */
+      ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f);
+      SetBit(state.accel_status, ACCEL_ECEF_I);
+      ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i);
+    }
+    else { /* could not get this representation,  set errno */
+      errno = 1;
+    }
+  } else { /* ned coordinate system not initialized,  set errno */
+    errno = 2;
+  }
+  if (errno) {
+    //struct NedCoor_i _ned_zero = {0};
+    //return _ned_zero;
   }
   /* set bit to indicate this representation is computed */
   SetBit(state.accel_status, ACCEL_NED_I);
@@ -541,12 +558,22 @@ void stateCalcAccelEcef_i(void) {
   if (bit_is_set(state.accel_status, ACCEL_ECEF_I))
     return;
 
-  if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
-    if (state.ned_initialised_i) {
-      //ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i);
-    }
-  } else {
-    //try floats....
+  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
+    ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f);
+  }
+  else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
+    ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i);
+  }
+  else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
+    /* transform ned_f -> ned_i -> ecef_i , set status bits */
+    ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f);
+    SetBit(state.accel_status, ACCEL_NED_I);
+    ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i);
+  }
+  else {
+    /* could not get this representation,  set errno */
+    //struct EcefCoor_i _ecef_zero = {0};
+    //return _ecef_zero;
   }
   /* set bit to indicate this representation is computed */
   SetBit(state.accel_status, ACCEL_ECEF_I);
@@ -558,12 +585,29 @@ void stateCalcAccelNed_f(void) {
   if (bit_is_set(state.accel_status, ACCEL_NED_F))
     return;
 
-  if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
-    if (state.ned_initialised_f) {
+  int errno = 0;
+  if (state.ned_initialised_f) {
+    if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
+      ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i);
+    }
+    else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) {
       ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f);
     }
-  } else {
-    //try ints....
+    else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
+      /* transform ecef_i -> ecef_f -> ned_f , set status bits */
+      ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ecef_accel_i);
+      SetBit(state.accel_status, ACCEL_ECEF_F);
+      ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f);
+    }
+    else { /* could not get this representation,  set errno */
+      errno = 1;
+    }
+  } else { /* ned coordinate system not initialized,  set errno */
+    errno = 2;
+  }
+  if (errno) {
+    //struct NedCoor_f _ned_zero = {0.0f};
+    //return _ned_zero;
   }
   /* set bit to indicate this representation is computed */
   SetBit(state.accel_status, ACCEL_NED_F);
@@ -575,10 +619,22 @@ void stateCalcAccelEcef_f(void) {
   if (bit_is_set(state.accel_status, ACCEL_ECEF_F))
     return;
 
-  if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
-    //ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f);
-  } else {
-    //try ints....
+  if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) {
+    ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ned_accel_i);
+  }
+  else if (bit_is_set(state.accel_status, ACCEL_NED_F)) {
+    ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f);
+  }
+  else if (bit_is_set(state.accel_status, ACCEL_NED_I)) {
+    /* transform ned_f -> ned_i -> ecef_i , set status bits */
+    ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i);
+    SetBit(state.accel_status, ACCEL_NED_F);
+    ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f);
+  }
+  else {
+    /* could not get this representation,  set errno */
+    //struct EcefCoor_f _ecef_zero = {0.0f};
+    //return _ecef_zero;
   }
   /* set bit to indicate this representation is computed */
   SetBit(state.accel_status, ACCEL_ECEF_F);