[ahrs] AllAhrsAligned check

paparazzi · Sep 2, 2015 · 2b67558 · 2b67558
1 parent 860f67a
commit 2b67558
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 3 deletions.
diff --git a/conf/flight_plans/rotorcraft_basic_superbitrf_from_hand.xml b/conf/flight_plans/rotorcraft_basic_superbitrf_from_hand.xml
@@ -56,7 +56,7 @@ The goal of this flightplan is to have a safe, simple no-brainer flightplan for
   <blocks>
     <block name="Init">
       <call fun="NavKillThrottle()"/>
-      <while cond="!GpsFixValid() || gps.pacc > 400 || !(ahrs.status == AHRS_RUNNING)"/>
+      <while cond="!GpsFixValid() || gps.pacc > 400 || !(ahrs_all_aligned())"/>
     </block>
     <block name="Geo init">
       <while cond="LessThan(NavBlockTime(), 8)"/>

diff --git a/sw/airborne/subsystems/ahrs.c b/sw/airborne/subsystems/ahrs.c
@@ -61,7 +61,7 @@ void ahrs_register_impl(AhrsEnableOutput enable, AhrsIsAligned aligned)
   for (i=0; i < AHRS_NB_IMPL; i++) {
     if (ahrs_impls[i].enable == NULL) {
       ahrs_impls[i].enable = enable;
-      ahrs_impls[i].enable = aligned;
+      ahrs_impls[i].aligned = aligned;
       break;
     }
   }

diff --git a/sw/airborne/subsystems/ahrs/ahrs_sim.c b/sw/airborne/subsystems/ahrs/ahrs_sim.c
@@ -60,9 +60,14 @@ void update_ahrs_from_sim(void)
 
 }
 
+static bool_t ahrs_sim_is_aligned(void)
+{
+  return TRUE;
+}
+
 
 void ahrs_sim_register(void)
 {
   // dummy, simple ocaml sim only supports one basic fake AHRS anyway
-  ahrs_register_impl(NULL);
+  ahrs_register_impl(NULL,ahrs_sim_is_aligned);
 }