[paparazzi-commits] [5299] Adding launch/land safety state machine

[Paul Cox]

Revision: 5299
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5299
Author:   paulcox
Date:     2010-08-10 18:29:59 +0000 (Tue, 10 Aug 2010)
Log Message:
-----------
Adding launch/land safety state machine

Modified Paths:
--------------
    paparazzi3/trunk/conf/settings/settings_beth.xml
    paparazzi3/trunk/sw/airborne/beth/main_overo.c
    paparazzi3/trunk/sw/airborne/beth/main_stm32.c
    paparazzi3/trunk/sw/airborne/beth/overo_controller.c
    paparazzi3/trunk/sw/airborne/beth/overo_controller.h
    paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c

Modified: paparazzi3/trunk/conf/settings/settings_beth.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_beth.xml    2010-08-10 14:32:58 UTC 
(rev 5298)
+++ paparazzi3/trunk/conf/settings/settings_beth.xml    2010-08-10 18:29:59 UTC 
(rev 5299)
@@ -2,11 +2,15 @@
 
 <settings>
   <dl_settings>
+    <dl_settings NAME="Controller">
 
-    <dl_settings NAME="Controller">
       <dl_setting var="controller.elevation_sp" min="-15" step="0.5" max="15" 
module="beth/overo_controller" shortname="elev_sp" unit="rad" alt_unit="deg" 
alt_unit_coef="57.32"/>
-      <dl_setting var="controller.tilt_sp" min="-15" step="0.5" max="15" 
module="beth/overo_controller"  shortname="tilt_sp" unit="rad" alt_unit="deg" 
alt_unit_coef="57.32"/>
-    </dl_settings>
 
+      <dl_setting var="controller.tilt_sp" min="-15" step="0.5" max="15" 
module="beth/overo_controller"  shortname="tilt_sp" unit="rad" alt_unit="deg" 
alt_unit_coef="57.32">
+      </dl_setting>
+
+      <dl_setting var="controller.armed" min="0" step="1" max="2" shortname 
="mode"/>
+
     </dl_settings>
+  </dl_settings>
 </settings>

Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-10 14:32:58 UTC 
(rev 5298)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-10 18:29:59 UTC 
(rev 5299)
@@ -59,7 +59,6 @@
 
 static uint32_t foo = 0;
 
-
 int main(int argc, char *argv[]) {
   
   (void) signal(SIGINT, main_exit);
@@ -100,6 +99,7 @@
 
 
 static void main_periodic(int my_sig_num) {
+  static last_state = 0;
 /*  static int bar=0;
   if (!(foo%2000)) {
     if (bar) {
@@ -109,6 +109,7 @@
     }
   }
 */
+  //if (foo >2000 ) { controller.armed=1;}
 
   RunOnceEvery(50, {DOWNLINK_SEND_ALIVE(gcs_com.udp_transport, 16, MD5SUM);});
  
@@ -122,10 +123,42 @@
   
estimator_run(msg_in.bench_sensor.z,msg_in.bench_sensor.y,msg_in.bench_sensor.x);
  
 
-  controller.elevation_sp = ((int32_t)(foo/2048.)%2) ? RadOfDeg(-20) : 
RadOfDeg(10);
+  switch (controller.armed) {
+    case 0:
+      if (last_state == 2) {
+        controller.armed = 2;
+        printf("Entering spinup mode from flight not permitted. Enter standby 
first.\n");
+        controller.elevation_sp = ((int32_t)(foo/2048.)%2) ? RadOfDeg(-20) : 
RadOfDeg(10);
+        control_run();
+      } else {
+        controller.cmd_pitch = 1;
+        controller.cmd_thrust = 1;
+        last_state=0;
+      }
+      break;
+    case 1:
+      if (last_state != 1) {
+        printf("Entering standby mode.\n");
+        controller.elevation_sp = RadOfDeg(-30);
+        controller.tilt_sp = 0;
+        last_state=1;
+      }
+      control_run();
+      break;
+    case 2:
+      if (last_state == 0) {
+        printf("Entering flight mode from spinup not permitted. Enter standby 
first.\n");
+        controller.armed = 0;
+      } else {
+        controller.elevation_sp = ((int32_t)(foo/2048.)%2) ? RadOfDeg(-20) : 
RadOfDeg(10);
+        control_run();
+        last_state=2;
+      }
+      break;
+    default:
+      break;
+  }
 
-  control_run();
-
   RunOnceEvery(25, {DOWNLINK_SEND_BETH_ESTIMATOR(gcs_com.udp_transport,
                        &estimator.tilt,&estimator.tilt_dot,
                        &estimator.elevation,&estimator.elevation_dot,
@@ -192,15 +225,29 @@
 
 
 static void main_exit(int sig) {
-  printf("Closing down\n");
+  printf("Initiating BETH shutdown...\n");
+
+//since the periodic event is no longer running when we get here right now,
+//this code doesn't do anything so removed for now.
+#if 0
+  printf("Zeroing setpoints...");
+  uint32_t startfoo = foo;
+  while (foo < (startfoo +2000)) {
+    controller.tilt_sp = 0;
+    controller.elevation_sp = RadOfDeg(-25);
+  }
+  printf("done\n");
+#endif
+  //If a logfile is being used, close it.
   //file_logger_exit()
+  printf("Main Overo Application Exiting...\n");
   exit(EXIT_SUCCESS);
 }
 
 
 static void main_talk_with_stm32() {
 
-  static int8_t adder = 1;
+  //static int8_t adder = 1;
   //uint8_t *fooptr;
 
   //msg_out.thrust = 0;

Modified: paparazzi3/trunk/sw/airborne/beth/main_stm32.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-08-10 14:32:58 UTC 
(rev 5298)
+++ paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-08-10 18:29:59 UTC 
(rev 5299)
@@ -70,7 +70,7 @@
 
 
 static inline void main_periodic( void ) {
-  int8_t pitch,thrust;
+  int8_t pitch;
   booz_imu_periodic();
 
   OveroLinkPeriodic(main_on_overo_link_lost)
@@ -90,7 +90,6 @@
   read_bench_sensors();
 
   pitch = (int8_t)((0xFF) & overo_link.down.msg.pitch);
-  thrust =(int8_t)((0xFF) & overo_link.down.msg.thrust);
 
   if (pitch > 10) pitch = 10; else 
    if (pitch < -10) pitch = -10; 
@@ -99,12 +98,12 @@
   booz2_commands[COMMAND_ROLL] = 0;
   booz2_commands[COMMAND_YAW] = 0;
   
-  if ( thrust < 100) {
-    booz2_commands[COMMAND_THRUST] = thrust;
+  if ( overo_link.down.msg.thrust < 100) {
+    booz2_commands[COMMAND_THRUST] = overo_link.down.msg.thrust;
   } else { 
     booz2_commands[COMMAND_THRUST] = 100;
   }
-  if ((my_cnt == 0) || ((pitch == 0) && (thrust == 0) )) {
+  if (my_cnt == 0) {
     actuators_set(FALSE);
   } else {
     actuators_set(TRUE);

Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-10 
14:32:58 UTC (rev 5298)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-10 
18:29:59 UTC (rev 5299)
@@ -39,6 +39,8 @@
 
   controller.cmd_pitch = 0.;
   controller.cmd_thrust = 0.;
+
+  controller.armed = 0;
 }
 
 

Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.h        2010-08-10 
14:32:58 UTC (rev 5298)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.h        2010-08-10 
18:29:59 UTC (rev 5299)
@@ -37,6 +37,8 @@
 
   float cmd_pitch;
   float cmd_thrust;
+
+  int armed;
 };
 
 

Modified: paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c   2010-08-10 14:32:58 UTC 
(rev 5298)
+++ paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c   2010-08-10 18:29:59 UTC 
(rev 5299)
@@ -9,7 +9,7 @@
 #include "dl_protocol.h"
 #include "settings.h"
 
-#define GCS_HOST "10.31.4.104"
+#define GCS_HOST "10.31.4.5"
 #define GCS_PORT 4242
 #define DATALINK_PORT 4243