git.gag.com Git - fw/altos/blob - src/kernel/ao_motor_flight.c

   1 /*
   2  * Copyright © 2022 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 #ifndef AO_FLIGHT_TEST
  20 #include "ao.h"
  21 #include <ao_log.h>
  22 #endif
  23
  24 #include <ao_flight.h>
  25
  26 /* Main flight thread. */
  27
  28 enum ao_flight_state    ao_flight_state;        /* current flight state */
  29 AO_TICK_TYPE            ao_launch_tick;         /* time of first boost detect */
  30
  31 #if HAS_SENSOR_ERRORS
  32 /* Any sensor can set this to mark the flight computer as 'broken' */
  33 uint8_t                 ao_sensor_errors;
  34 #endif
  35
  36 /*
  37  * track min/max data over a long interval to detect
  38  * resting
  39  */
  40 static AO_TICK_TYPE     ao_interval_end;
  41
  42 #define init_bounds(_cur, _min, _max) do {                              \
  43                 _min = _max = _cur;                                     \
  44         } while (0)
  45
  46 #define check_bounds(_cur, _min, _max) do {     \
  47                 if (_cur < _min)                \
  48                         _min = _cur;            \
  49                 if (_cur > _max)                \
  50                         _max = _cur;            \
  51         } while(0)
  52
  53 uint8_t                 ao_flight_force_idle;
  54
  55 /*
  56  * Landing is detected by getting constant readings from pressure sensor
  57  * for a fairly long time (AO_INTERVAL_TICKS), along with the max being
  58  * less than the boost detect pressure
  59  */
  60 #define AO_INTERVAL_TICKS       AO_SEC_TO_TICKS(10)
  61
  62 static AO_TICK_TYPE             ao_interval_end;
  63 static motor_pressure_t         ao_interval_min_motor_pressure, ao_interval_max_motor_pressure;
  64
  65 #define abs(a)  ((a) < 0 ? -(a) : (a))
  66
  67 void
  68 ao_flight(void)
  69 {
  70         ao_sample_init();
  71         ao_flight_state = ao_flight_startup;
  72         for (;;) {
  73
  74                 /*
  75                  * Process ADC samples, just looping
  76                  * until the sensors are calibrated.
  77                  */
  78                 if (!ao_sample())
  79                         continue;
  80
  81                 switch (ao_flight_state) {
  82                 case ao_flight_startup:
  83
  84                         if (!ao_flight_force_idle)
  85                         {
  86                                 /* Set pad mode - we can fly! */
  87                                 ao_flight_state = ao_flight_pad;
  88 #if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE && !DEBUG
  89                                 /* Disable the USB controller in flight mode
  90                                  * to save power
  91                                  */
  92 #if HAS_FAKE_FLIGHT
  93                                 if (!ao_fake_flight_active)
  94 #endif
  95                                         ao_usb_disable();
  96 #endif
  97
  98 #if AO_LED_RED
  99                                 /* signal successful initialization by turning off the LED */
 100                                 ao_led_off(AO_LED_RED);
 101 #endif
 102                         } else {
 103                                 /* Set idle mode */
 104                                 ao_flight_state = ao_flight_idle;
 105 #if HAS_SENSOR_ERRORS
 106                                 if (ao_sensor_errors)
 107                                         ao_flight_state = ao_flight_invalid;
 108 #endif
 109
 110 #if AO_LED_RED
 111                                 /* signal successful initialization by turning off the LED */
 112                                 ao_led_off(AO_LED_RED);
 113 #endif
 114                         }
 115                         /* wakeup threads due to state change */
 116                         ao_wakeup(&ao_flight_state);
 117
 118                         break;
 119
 120                 case ao_flight_pad:
 121                         /* pad to boost:
 122                          *
 123                          * motor pressure rise > 50psi
 124                          */
 125                         if (ao_sample_motor_pressure - ao_ground_motor_pressure >= AO_BOOST_DETECT)
 126                         {
 127                                 ao_flight_state = ao_flight_boost;
 128                                 ao_wakeup(&ao_flight_state);
 129
 130                                 ao_launch_tick = ao_sample_tick;
 131
 132                                 /* start logging data */
 133 #if HAS_LOG
 134                                 ao_log_start();
 135 #endif
 136                                 /* Initialize landing detection interval values */
 137                                 ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
 138
 139                                 init_bounds(ao_sample_motor_pressure, ao_interval_min_motor_pressure, ao_interval_max_motor_pressure);
 140                         }
 141                         break;
 142                 case ao_flight_boost:
 143                         /* boost to landed:
 144                          *
 145                          * motor pressure low and stable for more than 10 seconds
 146                          */
 147                         check_bounds(ao_sample_motor_pressure, ao_interval_min_motor_pressure,
 148                                      ao_interval_max_motor_pressure);
 149
 150                         if ((AO_TICK_SIGNED) (ao_sample_tick - ao_interval_end) >= 0) {
 151                                 if (ao_interval_max_motor_pressure - ao_ground_motor_pressure <= AO_BOOST_DETECT &&
 152                                     ao_interval_max_motor_pressure - ao_interval_min_motor_pressure <= AO_QUIET_DETECT_PRESSURE)
 153                                 {
 154                                         ao_flight_state = ao_flight_landed;
 155                                         ao_wakeup(&ao_flight_state);
 156 #if HAS_ADC
 157                                         /* turn off the ADC capture */
 158                                         ao_timer_set_adc_interval(0);
 159 #endif
 160                                 }
 161
 162                                 /* Reset interval values */
 163                                 ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS;
 164
 165                                 init_bounds(ao_sample_motor_pressure, ao_interval_min_motor_pressure, ao_interval_max_motor_pressure);
 166                         }
 167                         break;
 168                 default:
 169                         break;
 170                 }
 171         }
 172 }
 173
 174 #if HAS_FLIGHT_DEBUG
 175 static inline int int_part(ao_v_t i)    { return i >> 4; }
 176 static inline int frac_part(ao_v_t i)   { return ((i & 0xf) * 100 + 8) / 16; }
 177
 178 static void
 179 ao_flight_dump(void)
 180 {
 181 #if HAS_ACCEL
 182         ao_v_t  accel;
 183
 184         accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16;
 185 #endif
 186
 187         printf ("sample:\n");
 188         printf ("  tick        %d\n", ao_sample_tick);
 189 #if HAS_BARO
 190         printf ("  raw pres    %ld\n", ao_sample_pres);
 191 #endif
 192 #if HAS_ACCEL
 193         printf ("  raw accel   %d\n", ao_sample_accel);
 194 #endif
 195 #if HAS_BARO
 196         printf ("  ground pres %ld\n", ao_ground_pres);
 197         printf ("  ground alt  %ld\n", ao_ground_height);
 198 #endif
 199 #if HAS_ACCEL
 200         printf ("  raw accel   %d\n", ao_sample_accel);
 201         printf ("  groundaccel %d\n", ao_ground_accel);
 202         printf ("  accel_2g    %d\n", ao_accel_2g);
 203 #endif
 204
 205 #if HAS_BARO
 206         printf ("  alt         %ld\n", ao_sample_alt);
 207         printf ("  height      %ld\n", ao_sample_height);
 208 #endif
 209
 210 #if HAS_ACCEL
 211         printf ("  accel       %d.%02d\n", int_part(accel), frac_part(accel));
 212 #endif
 213
 214
 215         printf ("kalman:\n");
 216         printf ("  height      %ld\n", ao_height);
 217         printf ("  speed       %d.%02d\n", int_part(ao_speed), frac_part(ao_speed));
 218         printf ("  accel       %d.%02d\n", int_part(ao_accel), frac_part(ao_accel));
 219         printf ("  max_height  %ld\n", ao_max_height);
 220         printf ("  avg_height  %ld\n", ao_avg_height);
 221         printf ("  error_h     %ld\n", ao_error_h);
 222 #if !HAS_ACCEL
 223         printf ("  error_avg   %d\n", ao_error_h_sq_avg);
 224 #endif
 225 }
 226
 227 static void
 228 ao_gyro_test(void)
 229 {
 230         ao_flight_state = ao_flight_test;
 231         ao_getchar();
 232         ao_flight_state = ao_flight_idle;
 233 }
 234
 235 uint8_t ao_orient_test;
 236
 237 static void
 238 ao_orient_test_select(void)
 239 {
 240         ao_orient_test = !ao_orient_test;
 241         printf("orient test %d\n", ao_orient_test);
 242 }
 243
 244 const struct ao_cmds ao_flight_cmds[] = {
 245         { ao_flight_dump,       "F\0Dump flight status" },
 246         { ao_gyro_test,         "G\0Test gyro code" },
 247         { ao_orient_test_select,"O\0Test orientation code" },
 248         { 0, NULL },
 249 };
 250 #endif
 251
 252 static struct ao_task   flight_task;
 253
 254 void
 255 ao_flight_init(void)
 256 {
 257         ao_flight_state = ao_flight_startup;
 258 #if HAS_FLIGHT_DEBUG
 259         ao_cmd_register(&ao_flight_cmds[0]);
 260 #endif
 261         ao_add_task(&flight_task, ao_flight, "flight");
 262 }