git.gag.com Git - fw/altos/blob - ao-tools/ao-fakeflight/ao-fake-convert.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 "cc.h"
  19 #include <ao-fakeflight.h>
  20 #include <stdio.h>
  21 #include <math.h>
  22
  23 /* From mega serial 1246 */
  24
  25 struct ao_ms5607_prom   ao_ms5607_prom = {
  26         .reserved =  156,
  27         .sens =  44229,
  28         .off =  38634,
  29         .tcs =  27789,
  30         .tco =  25015,
  31         .tref =  32003,
  32         .tempsens =  27647,
  33         .crc =  50256,
  34 };
  35
  36 struct ao_config {
  37         int16_t accel_plus_g;
  38         int16_t accel_minus_g;
  39 };
  40
  41 struct ao_config ao_config = {
  42         .accel_plus_g = 2028,
  43         .accel_minus_g = 2065
  44 };
  45
  46 #define MIN_VALUE       0
  47 #define MAX_VALUE       16777216
  48 #define MID_VALUE       ((MAX_VALUE - MIN_VALUE) / 2)
  49
  50 static void
  51 ao_ms5607_search (struct ao_ms5607_value        *goal_value,
  52                   struct ao_ms5607_sample       *goal_sample,
  53                   int   search_temp)
  54 {
  55         struct ao_ms5607_sample sample;
  56         struct ao_ms5607_value  value;
  57         uint32_t                hi, lo, mid;
  58         int32_t                 goal;
  59         int32_t                 result;
  60
  61         if (search_temp) {
  62                 sample.pres = MID_VALUE;
  63                 goal = goal_value->temp;
  64         } else {
  65                 sample.temp = goal_sample->temp;
  66                 goal = goal_value->pres;
  67         }
  68
  69         lo = MIN_VALUE;
  70         hi = MAX_VALUE;
  71         for (;;) {
  72                 mid = (hi + lo) >> 1;
  73
  74                 if (mid == lo)
  75                         break;
  76
  77                 if (search_temp)
  78                         sample.temp = mid;
  79                 else
  80                         sample.pres = mid;
  81
  82                 ao_ms5607_convert(&sample, &value);
  83
  84                 if (search_temp)
  85                         result = value.temp;
  86                 else
  87                         result = value.pres;
  88
  89                 if (result == goal)
  90                         break;
  91
  92                 if (result < goal)
  93                         lo = mid;
  94                 else
  95                         hi = mid;
  96         }
  97         *goal_sample = sample;
  98 }
  99
 100 void
 101 ao_ms5607_unconvert(double altitude,
 102                     struct ao_ms5607_sample *ret)
 103 {
 104         double pressure = cc_altitude_to_pressure(altitude);
 105         double temperature = cc_altitude_to_temperature(altitude);
 106
 107         struct ao_ms5607_value  value;
 108
 109         value.pres = floor (pressure + 0.5);
 110         value.temp = floor (temperature * 100 + 0.5);
 111
 112         /* First back-convert temperature; that's independent of pressure */
 113
 114         ao_ms5607_search(&value, ret, 1);
 115
 116         /* Now search for pressure */
 117
 118         ao_ms5607_search(&value, ret, 0);
 119 }
 120
 121 int16_t
 122 ao_accel_unconvert(double accel)
 123 {
 124         int16_t ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g;
 125         double  ao_accel_0g = (ao_config.accel_minus_g + ao_config.accel_plus_g) / 2.0;
 126         double  ao_accel_scale = GRAVITY * 2.0 / ao_accel_2g;
 127
 128         int16_t ao_sample_accel = ao_accel_0g - accel/ao_accel_scale;
 129
 130         return ao_sample_accel;
 131 }
 132
 133 static void
 134 ao_ms5607_show(FILE *f, struct rocket *r)
 135 {
 136         fprintf(f, "ms5607 reserved: %u\n", ao_ms5607_prom.reserved);
 137         fprintf(f, "ms5607 sens: %u\n", ao_ms5607_prom.sens);
 138         fprintf(f, "ms5607 off: %u\n", ao_ms5607_prom.off);
 139         fprintf(f, "ms5607 tcs: %u\n", ao_ms5607_prom.tcs);
 140         fprintf(f, "ms5607 tco: %u\n", ao_ms5607_prom.tco);
 141         fprintf(f, "ms5607 tref: %u\n", ao_ms5607_prom.tref);
 142         fprintf(f, "ms5607 tempsens: %u\n", ao_ms5607_prom.tempsens);
 143         fprintf(f, "ms5607 crc: %u\n", ao_ms5607_prom.crc);
 144 }
 145
 146 static void
 147 ao_config_show(FILE *f, struct rocket *r)
 148 {
 149         fprintf(f, "Config version: 1.18\n");
 150         fprintf(f, "Accel cal +1g: %d -1g: %d\n",
 151                 ao_config.accel_plus_g, ao_config.accel_minus_g);
 152         fprintf(f, "Main deploy: %d meters\n", (int) r->main_deploy);
 153 }
 154
 155 #include "config.h"
 156
 157 void
 158 ao_show_header(FILE *f, struct rocket *r)
 159 {
 160         ao_config_show(f, r);
 161         ao_ms5607_show(f, r);
 162         fprintf(f, "serial-number 1\n");
 163         fprintf(f, "current-flight 1\n");
 164         fprintf(f, "log-format       %u\n", AO_LOG_FORMAT_TELEMETRUM);
 165         fprintf(f, "altitude-32      1\n");
 166         fprintf(f, "software-version %s\n", PACKAGE_VERSION);
 167 }
 168
 169 #include "ao_ms5607_convert.c"