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

   1 /*
   2  * Copyright © 2014 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; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #include <ao.h>
  19 #include <ao_fake_flight.h>
  20 #if HAS_MS5607 || HAS_MS5611
  21 #include <ao_ms5607.h>
  22 #endif
  23
  24 uint8_t                 ao_fake_flight_active;
  25
  26 static uint8_t          ao_fake_has_cur;
  27 static volatile uint8_t ao_fake_has_next;
  28 static uint8_t          ao_fake_has_offset;
  29 static uint16_t         ao_fake_tick_offset;
  30 static struct ao_data   ao_fake_cur, ao_fake_next;
  31
  32 void
  33 ao_fake_flight_poll(void)
  34 {
  35         if (ao_fake_has_next && (ao_tick_count - ao_fake_next.tick) >= 0) {
  36                 ao_fake_cur = ao_fake_next;
  37                 ao_fake_has_next = 0;
  38                 ao_wakeup((void *) &ao_fake_has_next);
  39                 ao_fake_has_cur = 1;
  40         }
  41         if (!ao_fake_has_cur)
  42                 return;
  43         ao_data_ring[ao_data_head] = ao_fake_cur;
  44         ao_data_ring[ao_data_head].tick = ao_tick_count;
  45         ao_data_head = ao_data_ring_next(ao_data_head);
  46         ao_wakeup((void *) &ao_data_head);
  47 }
  48
  49 static uint8_t
  50 ao_fake_data_read(void)
  51 {
  52         uint8_t i;
  53         uint8_t *d = (void *) &ao_fake_next;
  54
  55         if (getchar() == 0)
  56                 return FALSE;
  57         for (i = 0; i < sizeof (struct ao_data); i++)
  58                 *d++ = getchar();
  59         if (!ao_fake_has_offset) {
  60                 ao_fake_tick_offset = (ao_tick_count + 1000) - ao_fake_next.tick;
  61                 ao_fake_next.tick = ao_tick_count;
  62                 ao_fake_has_offset = 1;
  63         } else
  64                 ao_fake_next.tick += ao_fake_tick_offset;
  65         ao_fake_has_next = 1;
  66         return TRUE;
  67 }
  68
  69 static void
  70 ao_fake_calib_get(struct ao_fake_calib *calib)
  71 {
  72 #if HAS_ACCEL
  73         calib->accel_plus_g = ao_config.accel_plus_g;
  74         calib->accel_minus_g = ao_config.accel_minus_g;
  75 #endif
  76 #if HAS_GYRO
  77         calib->accel_zero_along = ao_config.accel_zero_along;
  78         calib->accel_zero_across = ao_config.accel_zero_across;
  79         calib->accel_zero_through = ao_config.accel_zero_through;
  80 #endif
  81 #if HAS_MS5607 || HAS_MS5611
  82         calib->ms5607_prom = ao_ms5607_prom;
  83 #endif
  84 }
  85
  86 static void
  87 ao_fake_calib_set(struct ao_fake_calib *calib)
  88 {
  89 #if HAS_ACCEL
  90         ao_config.accel_plus_g = calib->accel_plus_g;
  91         ao_config.accel_minus_g = calib->accel_minus_g;
  92 #endif
  93 #if HAS_GYRO
  94         ao_config.accel_zero_along = calib->accel_zero_along;
  95         ao_config.accel_zero_across = calib->accel_zero_across;
  96         ao_config.accel_zero_through = calib->accel_zero_through;
  97 #endif
  98 #if HAS_MS5607 || HAS_MS5611
  99         ao_ms5607_prom = calib->ms5607_prom;
 100 #endif
 101 }
 102
 103 static uint8_t
 104 ao_fake_calib_read(void)
 105 {
 106         struct ao_fake_calib    ao_calib;
 107         uint8_t                 *d = (void *) &ao_calib;
 108         uint16_t                i;
 109
 110         /* Read calibration data */
 111         for (i = 0; i < sizeof (struct ao_fake_calib); i++)
 112                 *d++ = getchar();
 113         if (ao_calib.major != AO_FAKE_CALIB_MAJOR
 114 #if AO_FAKE_CALIB_MINOR != 0
 115             || ao_calib.minor < AO_FAKE_CALIB_MINOR
 116 #endif
 117                 ) {
 118                 printf ("Calibration data major version mismatch %d.%d <= %d.%d\n",
 119                         ao_calib.major, ao_calib.minor, AO_FAKE_CALIB_MAJOR, AO_FAKE_CALIB_MINOR);
 120                 return FALSE;
 121         }
 122         ao_fake_calib_set(&ao_calib);
 123         return TRUE;
 124 }
 125
 126 static void
 127 ao_fake_flight(void)
 128 {
 129         int16_t                 calib_size, data_size;
 130         struct ao_fake_calib    save_calib;
 131         uint16_t                my_pyro_fired = 0;
 132         enum ao_flight_state    my_state = ao_flight_invalid;
 133         int                     i;
 134
 135         ao_cmd_hex();
 136         if (ao_cmd_status != ao_cmd_success)
 137                 return;
 138         calib_size = ao_cmd_lex_i;
 139         ao_cmd_hex();
 140         if (ao_cmd_status != ao_cmd_success)
 141                 return;
 142         data_size = ao_cmd_lex_i;
 143         if ((unsigned) calib_size != sizeof (struct ao_fake_calib)) {
 144                 printf ("calib size %d larger than actual size %d\n",
 145                         calib_size, sizeof (struct ao_fake_calib));
 146                 ao_cmd_status = ao_cmd_syntax_error;
 147                 return;
 148         }
 149         if (data_size != sizeof (struct ao_data)) {
 150                 printf ("data size %d doesn't match actual size %d\n",
 151                         data_size, sizeof (struct ao_data));
 152                 ao_cmd_status = ao_cmd_syntax_error;
 153                 return;
 154         }
 155         ao_fake_calib_get(&save_calib);
 156         if (!ao_fake_calib_read())
 157                 return;
 158
 159         ao_fake_has_next = 0;
 160         ao_fake_has_cur = 0;
 161         ao_fake_flight_active = 1;
 162         ao_sample_init();
 163 #if PACKET_HAS_SLAVE
 164         ao_packet_slave_stop();
 165 #endif
 166 #if AO_LED_RED
 167         /* Turn on the LED to indicate startup */
 168         ao_led_on(AO_LED_RED);
 169 #endif
 170         ao_flight_state = ao_flight_startup;
 171         for (;;) {
 172                 if (my_state != ao_flight_state) {
 173                         printf("state %d\n", ao_flight_state);
 174                         my_state = ao_flight_state;
 175                         flush();
 176                 }
 177                 if (my_pyro_fired != ao_pyro_fired) {
 178                         int     pyro;
 179
 180                         for (pyro = 0; pyro < AO_PYRO_NUM; pyro++) {
 181                                 uint16_t        bit = (1 << pyro);
 182                                 if (!(my_pyro_fired & bit) && (ao_pyro_fired & bit))
 183                                         printf ("fire %d\n", pyro);
 184                         }
 185                         my_pyro_fired = ao_pyro_fired;
 186                 }
 187                 while (ao_fake_has_next)
 188                         ao_sleep((void *) &ao_fake_has_next);
 189                 if (!ao_fake_data_read())
 190                         break;
 191         }
 192
 193         /* Wait 20 seconds to see if we enter landed state */
 194         for (i = 0; i < 200; i++)
 195         {
 196                 if (ao_flight_state == ao_flight_landed)
 197                         break;
 198                 ao_delay(AO_MS_TO_TICKS(100));
 199         }
 200 #if AO_LED_RED
 201         /* Turn on the LED to indicate startup */
 202         ao_led_on(AO_LED_RED);
 203 #endif
 204         ao_fake_flight_active = 0;
 205         ao_flight_state = ao_flight_startup;
 206         ao_sample_init();
 207         ao_fake_calib_set(&save_calib);
 208 }
 209
 210 static const struct ao_cmds ao_fake_flight_cmds[] = {
 211         { ao_fake_flight,       "F <calib-size> <data-size>\0Start fake flight" },
 212         { 0, NULL }
 213 };
 214
 215 void
 216 ao_fake_flight_init(void)
 217 {
 218         ao_cmd_register(&ao_fake_flight_cmds[0]);
 219 }