diff options
Diffstat (limited to 'src/drivers')
| -rw-r--r-- | src/drivers/ao_aprs.c | 204 | ||||
| -rw-r--r-- | src/drivers/ao_cc115l.c | 37 | ||||
| -rw-r--r-- | src/drivers/ao_gps_ublox.c | 13 | ||||
| -rw-r--r-- | src/drivers/ao_lco_func.c | 2 | ||||
| -rw-r--r-- | src/drivers/ao_ms5607.c | 22 | ||||
| -rw-r--r-- | src/drivers/ao_ms5607.h | 4 | ||||
| -rw-r--r-- | src/drivers/ao_ms5607_convert.c | 12 | ||||
| -rw-r--r-- | src/drivers/ao_ms5607_convert_8051.c | 20 | ||||
| -rw-r--r-- | src/drivers/ao_pad.c | 6 | ||||
| -rw-r--r-- | src/drivers/ao_pca9922.c | 21 | ||||
| -rw-r--r-- | src/drivers/ao_quadrature.c | 103 |
11 files changed, 324 insertions, 120 deletions
diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index c327c897..8a1b6a4d 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -144,7 +144,6 @@ #endif #include <ao_aprs.h> -#include <math.h> // Public methods, constants, and data structures for each class. @@ -500,40 +499,205 @@ static int ao_num_sats(void) return n; } +static char ao_gps_locked(void) +{ + if (ao_gps_data.flags & AO_GPS_VALID) + return 'L'; + else + return 'U'; +} + static int tncComment(uint8_t *buf) { #if HAS_ADC struct ao_data packet; - + ao_arch_critical(ao_data_get(&packet);); int16_t battery = ao_battery_decivolt(packet.adc.v_batt); +#ifdef AO_SENSE_DROGUE int16_t apogee = ao_ignite_decivolt(AO_SENSE_DROGUE(&packet)); +#endif +#ifdef AO_SENSE_MAIN int16_t main = ao_ignite_decivolt(AO_SENSE_MAIN(&packet)); +#endif return sprintf((char *) buf, - "S: %d B:%d.%d A:%d.%d M:%d.%d", + "%c%d B%d.%d" +#ifdef AO_SENSE_DROGUE + " A%d.%d" +#endif +#ifdef AO_SENSE_MAIN + " M%d.%d" +#endif + , ao_gps_locked(), ao_num_sats(), battery/10, - battery % 10, - apogee/10, - apogee%10, - main/10, - main%10); + battery % 10 +#ifdef AO_SENSE_DROGUE + , apogee/10, + apogee%10 +#endif +#ifdef AO_SENSE_MAIN + , main/10, + main%10 +#endif + ); #else return sprintf((char *) buf, - "S: %d", ao_num_sats()); + "%c%d", + ao_gps_locked(), + ao_num_sats()); #endif } +/* + * APRS use a log encoding of altitude with a base of 1.002, such that + * + * feet = 1.002 ** encoded_altitude + * + * meters = (1.002 ** encoded_altitude) * 0.3048 + * + * log2(meters) = log2(1.002 ** encoded_altitude) + log2(0.3048) + * + * log2(meters) = encoded_altitude * log2(1.002) + log2(0.3048) + * + * encoded_altitude = (log2(meters) - log2(0.3048)) / log2(1.002) + * + * encoded_altitude = (log2(meters) + log2(1/0.3048)) * (1/log2(1.002)) + * + * We need 9 bits of mantissa to hold 1/log2(1.002) (~ 347), which leaves us + * 23 bits of fraction. That turns out to be *just* enough to avoid any + * errors in the result (cool, huh?). + */ + +#define fixed23_int(x) ((uint32_t) ((x) << 23)) +#define fixed23_one fixed23_int(1) +#define fixed23_two fixed23_int(2) +#define fixed23_half (fixed23_one >> 1) +#define fixed23_floor(x) ((x) >> 23) +#define fixed23_real(x) ((uint32_t) ((x) * fixed23_one + 0.5)) + +static inline uint64_t +fixed23_mul(uint32_t x, uint32_t y) +{ + return ((uint64_t) x * y + fixed23_half) >> 23; +} + +/* + * Use 30 fraction bits for the altitude. We need two bits at the + * top as we need to handle x, where 0 <= x < 4. We don't + * need 30 bits, but it's actually easier this way as we normalize + * the incoming value to 1 <= x < 2, and having the integer portion + * way up high means we don't have to deal with shifting in both + * directions to cover from 0 to 2**30-1. + */ + +#define fixed30_int(x) ((uint32_t) ((x) << 30)) +#define fixed30_one fixed30_int(1) +#define fixed30_half (fixed30_one >> 1) +#define fixed30_two fixed30_int(2) + +static inline uint32_t +fixed30_mul(uint32_t x, uint32_t y) +{ + return ((uint64_t) x * y + fixed30_half) >> 30; +} + +/* + * Fixed point log2. Takes integer argument, returns + * fixed point result with 23 bits of fraction + */ + +static uint32_t +ao_fixed_log2(uint32_t x) +{ + uint32_t result; + uint32_t frac = fixed23_one; + + /* Bounds check for sanity */ + if (x <= 0) + return 0; + + if (x >= fixed30_one) + return 0xffffffff; + + /* + * Normalize and compute integer log portion + * + * This makes 1 <= x < 2, and computes result to be + * the integer portion of the log2 of x + */ + + for (result = fixed23_int(30); x < fixed30_one; result -= fixed23_one, x <<= 1) + ; + + /* + * Given x, find y and n such that: + * + * x = y * 2**n 1 <= y < 2 + * + * That means: + * + * lb(x) = n + lb(y) + * + * Now, repeatedly square y to find find z and m such that: + * + * z = y ** (2**m) 2 <= z < 4 + * + * This is possible because 1 <= y < 2 + * + * lb(y) = lb(z) / 2**m + * + * (1 + lb(z/2)) + * = ------------- + * 2**m + * + * = 2**-m + 2**-m * lb(z/2) + * + * Note that if 2 <= z < 4, then 1 <= (z/2) < 2, so we can + * iterate to find lb(z/2) + * + * In this implementation, we don't care about the 'm' value, + * instead we only care about 2**-m, which we store in 'frac' + */ + + while (frac != 0 && x != fixed30_one) { + /* Repeatedly square x until 2 <= x < 4 */ + while (x < fixed30_two) { + x = fixed30_mul(x, x); + + /* Divide the fractional result bit by 2 */ + frac >>= 1; + } + + /* Add in this result bit */ + result |= frac; + + /* Make 1 <= x < 2 again and iterate */ + x >>= 1; + } + return result; +} + +#define APRS_LOG_CONVERT fixed23_real(1.714065192056127) +#define APRS_LOG_BASE fixed23_real(346.920048461100941) + +static int +ao_aprs_encode_altitude(int meters) +{ + return fixed23_floor(fixed23_mul(ao_fixed_log2(meters) + APRS_LOG_CONVERT, APRS_LOG_BASE) + fixed23_half); +} + /** * Generate the plain text position packet. */ static int tncPositionPacket(void) { - int32_t latitude = 0; - int32_t longitude = 0; - int32_t altitude = 0; + static int32_t latitude; + static int32_t longitude; + static int32_t altitude; + int32_t lat, lon, alt; uint8_t *buf; if (ao_gps_data.flags & AO_GPS_VALID) { @@ -544,30 +708,26 @@ static int tncPositionPacket(void) altitude = 0; } - altitude = (altitude * (int32_t) 10000 + (3048/2)) / (int32_t) 3048; - buf = tncBuffer; *buf++ = '!'; /* Symbol table ID */ *buf++ = '/'; - latitude = ((uint64_t) 380926 * (900000000 - latitude)) / 10000000; - longitude = ((uint64_t) 190463 * (1800000000 + longitude)) / 10000000; - -#define ALTITUDE_LOG_BASE 0.001998002662673f /* log(1.002) */ + lat = ((uint64_t) 380926 * (900000000 - latitude)) / 10000000; + lon = ((uint64_t) 190463 * (1800000000 + longitude)) / 10000000; - altitude = logf((float) altitude) * (1/ALTITUDE_LOG_BASE); + alt = ao_aprs_encode_altitude(altitude); - tncCompressInt(buf, latitude, 4); + tncCompressInt(buf, lat, 4); buf += 4; - tncCompressInt(buf, longitude, 4); + tncCompressInt(buf, lon, 4); buf += 4; /* Symbol code */ *buf++ = '\''; - tncCompressInt(buf, altitude, 2); + tncCompressInt(buf, alt, 2); buf += 2; *buf++ = 33 + ((1 << 5) | (2 << 3)); diff --git a/src/drivers/ao_cc115l.c b/src/drivers/ao_cc115l.c index f0f72d4d..f250e714 100644 --- a/src/drivers/ao_cc115l.c +++ b/src/drivers/ao_cc115l.c @@ -52,8 +52,8 @@ struct ao_cc115l_reg { #if CC115L_TRACE -const static struct ao_cc115l_reg ao_cc115l_reg[]; -const static char *cc115l_state_name[]; +static const struct ao_cc115l_reg ao_cc115l_reg[]; +static const char *cc115l_state_name[]; enum ao_cc115l_trace_type { trace_strobe, @@ -88,6 +88,8 @@ static void trace_add(enum ao_cc115l_trace_type type, int16_t addr, int16_t valu case trace_strobe: comment = cc115l_state_name[(value >> 4) & 0x7]; break; + default: + break; } trace[trace_i].type = type; trace[trace_i].addr = addr; @@ -197,24 +199,22 @@ ao_radio_tx_fifo_space(void) return CC115L_FIFO_SIZE - (ao_radio_reg_read(CC115L_TXBYTES) & CC115L_TXBYTES_NUM_TX_BYTES_MASK); } -#if UNUSED +#if CC115L_DEBUG static uint8_t ao_radio_status(void) { return ao_radio_strobe (CC115L_SNOP); } -#endif -#define ao_radio_rdf_value 0x55 - -#if UNUSED static uint8_t ao_radio_get_marcstate(void) { return ao_radio_reg_read(CC115L_MARCSTATE) & CC115L_MARCSTATE_MASK; } #endif - + +#define ao_radio_rdf_value 0x55 + static void ao_radio_done_isr(void) { @@ -612,6 +612,12 @@ ao_radio_rdf_abort(void) #define POWER_STEP 0x08 +#if HAS_RADIO_POWER +#define RADIO_POWER ao_config.radio_power +#else +#define RADIO_POWER 0xc0 +#endif + static void ao_radio_stx(void) { @@ -619,9 +625,10 @@ ao_radio_stx(void) ao_radio_pa_on(); ao_radio_reg_write(CC115L_PA, 0); ao_radio_strobe(CC115L_STX); - for (power = POWER_STEP; power < ao_config.radio_power; power += POWER_STEP) + for (power = POWER_STEP; power < RADIO_POWER; power += POWER_STEP) ao_radio_reg_write(CC115L_PA, power); - ao_radio_reg_write(CC115L_PA, ao_config.radio_power); + if (power != RADIO_POWER) + ao_radio_reg_write(CC115L_PA, RADIO_POWER); } static void @@ -668,8 +675,8 @@ ao_radio_test_cmd(void) static inline int16_t ao_radio_gpio_bits(void) { - return AO_CC115L_DONE_INT_PORT->idr & ((1 << AO_CC115L_FIFO_INT_PIN) | - (1 << AO_CC115L_DONE_INT_PIN)); + return ((ao_gpio_get(AO_CC115L_DONE_INT_PORT, AO_CC115L_DONE_INT_PIN, AO_CC115L_DONE_INT) << 1) | + ao_gpio_get(AO_CC115L_FIFO_INT_PORT, AO_CC115L_FIFO_INT_PIN, AO_CC115L_FIFO_INT)); } #endif @@ -819,7 +826,7 @@ ao_radio_send_aprs(ao_radio_fill_func fill) } #if CC115L_DEBUG -const static char *cc115l_state_name[] = { +static const char *cc115l_state_name[] = { [CC115L_STATUS_STATE_IDLE] = "IDLE", [CC115L_STATUS_STATE_TX] = "TX", [CC115L_STATUS_STATE_FSTXON] = "FSTXON", @@ -828,7 +835,7 @@ const static char *cc115l_state_name[] = { [CC115L_STATUS_STATE_TX_FIFO_UNDERFLOW] = "TX_FIFO_UNDERFLOW", }; -const static struct ao_cc115l_reg ao_cc115l_reg[] = { +static const struct ao_cc115l_reg ao_cc115l_reg[] = { { .addr = CC115L_IOCFG2, .name = "IOCFG2" }, { .addr = CC115L_IOCFG1, .name = "IOCFG1" }, { .addr = CC115L_IOCFG0, .name = "IOCFG0" }, @@ -874,7 +881,7 @@ const static struct ao_cc115l_reg ao_cc115l_reg[] = { static void ao_radio_show(void) { uint8_t status = ao_radio_status(); - int i; + unsigned int i; ao_radio_get(); status = ao_radio_status(); diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 01169522..077698a9 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -601,6 +601,14 @@ static const uint8_t ublox_enable_nav[] = { }; void +ao_gps_set_rate(uint8_t rate) +{ + uint8_t i; + for (i = 0; i < sizeof (ublox_enable_nav); i++) + ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], rate); +} + +void ao_gps(void) __reentrant { uint8_t class, id; @@ -616,8 +624,7 @@ ao_gps(void) __reentrant ao_ublox_set_message_rate(UBLOX_NAV, ublox_disable_nav[i], 0); /* Enable all of the messages we want */ - for (i = 0; i < sizeof (ublox_enable_nav); i++) - ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], 1); + ao_gps_set_rate(1); ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE), UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G, @@ -713,7 +720,7 @@ ao_gps(void) __reentrant ao_gps_data.flags |= AO_GPS_RUNNING; if (nav_sol.gps_fix & (1 << NAV_SOL_FLAGS_GPSFIXOK)) { uint8_t nsat = nav_sol.nsat; - ao_gps_data.flags |= AO_GPS_VALID; + ao_gps_data.flags |= AO_GPS_VALID | AO_GPS_COURSE_VALID; if (nsat > 15) nsat = 15; ao_gps_data.flags |= nsat; diff --git a/src/drivers/ao_lco_func.c b/src/drivers/ao_lco_func.c index a5d28e61..9e642836 100644 --- a/src/drivers/ao_lco_func.c +++ b/src/drivers/ao_lco_func.c @@ -36,7 +36,7 @@ ao_lco_query(uint16_t box, struct ao_pad_query *query, uint16_t *tick_offset) command.channels = 0; ao_radio_cmac_send(&command, sizeof (command)); sent_time = ao_time(); - r = ao_radio_cmac_recv(query, sizeof (*query), AO_MS_TO_TICKS(20)); + r = ao_radio_cmac_recv(query, sizeof (*query), AO_MS_TO_TICKS(10)); if (r == AO_RADIO_CMAC_OK) *tick_offset = sent_time - query->tick; ao_mutex_put(&ao_lco_mutex); diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c index 58ab9197..6098699e 100644 --- a/src/drivers/ao_ms5607.c +++ b/src/drivers/ao_ms5607.c @@ -21,8 +21,8 @@ #if HAS_MS5607 || HAS_MS5611 -static __xdata struct ao_ms5607_prom ms5607_prom; -static __xdata uint8_t ms5607_configured; +__xdata struct ao_ms5607_prom ao_ms5607_prom; +static __xdata uint8_t ms5607_configured; static void ao_ms5607_start(void) { @@ -111,7 +111,7 @@ ao_ms5607_setup(void) return; ms5607_configured = 1; ao_ms5607_reset(); - ao_ms5607_prom_read(&ms5607_prom); + ao_ms5607_prom_read(&ao_ms5607_prom); } static __xdata volatile uint8_t ao_ms5607_done; @@ -208,14 +208,14 @@ __xdata struct ao_task ao_ms5607_task; void ao_ms5607_info(void) { - printf ("ms5607 reserved: %u\n", ms5607_prom.reserved); - printf ("ms5607 sens: %u\n", ms5607_prom.sens); - printf ("ms5607 off: %u\n", ms5607_prom.off); - printf ("ms5607 tcs: %u\n", ms5607_prom.tcs); - printf ("ms5607 tco: %u\n", ms5607_prom.tco); - printf ("ms5607 tref: %u\n", ms5607_prom.tref); - printf ("ms5607 tempsens: %u\n", ms5607_prom.tempsens); - printf ("ms5607 crc: %u\n", ms5607_prom.crc); + printf ("ms5607 reserved: %u\n", ao_ms5607_prom.reserved); + printf ("ms5607 sens: %u\n", ao_ms5607_prom.sens); + printf ("ms5607 off: %u\n", ao_ms5607_prom.off); + printf ("ms5607 tcs: %u\n", ao_ms5607_prom.tcs); + printf ("ms5607 tco: %u\n", ao_ms5607_prom.tco); + printf ("ms5607 tref: %u\n", ao_ms5607_prom.tref); + printf ("ms5607 tempsens: %u\n", ao_ms5607_prom.tempsens); + printf ("ms5607 crc: %u\n", ao_ms5607_prom.crc); } static void diff --git a/src/drivers/ao_ms5607.h b/src/drivers/ao_ms5607.h index 206efd64..b58178fd 100644 --- a/src/drivers/ao_ms5607.h +++ b/src/drivers/ao_ms5607.h @@ -57,6 +57,7 @@ struct ao_ms5607_value { }; extern __xdata struct ao_ms5607_sample ao_ms5607_current; +extern __xdata struct ao_ms5607_prom ao_ms5607_prom; void ao_ms5607_setup(void); @@ -74,7 +75,4 @@ void ao_ms5607_convert(__xdata struct ao_ms5607_sample *sample, __xdata struct ao_ms5607_value *value); -void -ao_ms5607_get_prom(__data struct ao_ms5607_prom *prom); - #endif /* _AO_MS5607_H_ */ diff --git a/src/drivers/ao_ms5607_convert.c b/src/drivers/ao_ms5607_convert.c index bfb952a4..4d412cbe 100644 --- a/src/drivers/ao_ms5607_convert.c +++ b/src/drivers/ao_ms5607_convert.c @@ -25,16 +25,16 @@ ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value int64_t OFF; int64_t SENS; - dT = sample->temp - ((int32_t) ms5607_prom.tref << 8); + dT = sample->temp - ((int32_t) ao_ms5607_prom.tref << 8); - TEMP = 2000 + (((int64_t) dT * ms5607_prom.tempsens) >> 23); + TEMP = 2000 + (((int64_t) dT * ao_ms5607_prom.tempsens) >> 23); #if HAS_MS5611 - OFF = ((int64_t) ms5607_prom.off << 16) + (((int64_t) ms5607_prom.tco * dT) >> 7); - SENS = ((int64_t) ms5607_prom.sens << 15) + (((int64_t) ms5607_prom.tcs * dT) >> 8); + OFF = ((int64_t) ao_ms5607_prom.off << 16) + (((int64_t) ao_ms5607_prom.tco * dT) >> 7); + SENS = ((int64_t) ao_ms5607_prom.sens << 15) + (((int64_t) ao_ms5607_prom.tcs * dT) >> 8); #else - OFF = ((int64_t) ms5607_prom.off << 17) + (((int64_t) ms5607_prom.tco * dT) >> 6); - SENS = ((int64_t) ms5607_prom.sens << 16) + (((int64_t) ms5607_prom.tcs * dT) >> 7); + OFF = ((int64_t) ao_ms5607_prom.off << 17) + (((int64_t) ao_ms5607_prom.tco * dT) >> 6); + SENS = ((int64_t) ao_ms5607_prom.sens << 16) + (((int64_t) ao_ms5607_prom.tcs * dT) >> 7); #endif if (TEMP < 2000) { diff --git a/src/drivers/ao_ms5607_convert_8051.c b/src/drivers/ao_ms5607_convert_8051.c index f3a48c46..a74086d9 100644 --- a/src/drivers/ao_ms5607_convert_8051.c +++ b/src/drivers/ao_ms5607_convert_8051.c @@ -40,30 +40,30 @@ ao_ms5607_convert(__xdata struct ao_ms5607_sample *sample, __LOCAL ao_int64_t SENS; __LOCAL ao_int64_t a; - dT = sample->temp - ((int32_t) ms5607_prom.tref << 8); + dT = sample->temp - ((int32_t) ao_ms5607_prom.tref << 8); - /* TEMP = 2000 + (((int64_t) dT * ms5607_prom.tempsens) >> 23); */ - ao_mul64_32_32(&a, dT, ms5607_prom.tempsens); + /* TEMP = 2000 + (((int64_t) dT * ao_ms5607_prom.tempsens) >> 23); */ + ao_mul64_32_32(&a, dT, ao_ms5607_prom.tempsens); ao_rshift64(&a, &a, 23); TEMP = 2000 + a.low; /* */ - /* OFF = ((int64_t) ms5607_prom.off << SHIFT_OFF) + (((int64_t) ms5607_prom.tco * dT) >> SHIFT_TCO);*/ + /* OFF = ((int64_t) ao_ms5607_prom.off << SHIFT_OFF) + (((int64_t) ao_ms5607_prom.tco * dT) >> SHIFT_TCO);*/ #if SHIFT_OFF > 16 - OFF.high = ms5607_prom.off >> (32 - SHIFT_OFF); + OFF.high = ao_ms5607_prom.off >> (32 - SHIFT_OFF); #else OFF.high = 0; #endif - OFF.low = (uint32_t) ms5607_prom.off << SHIFT_OFF; - ao_mul64_32_32(&a, ms5607_prom.tco, dT); + OFF.low = (uint32_t) ao_ms5607_prom.off << SHIFT_OFF; + ao_mul64_32_32(&a, ao_ms5607_prom.tco, dT); ao_rshift64(&a, &a, SHIFT_TCO); ao_plus64(&OFF, &OFF, &a); /**/ - /* SENS = ((int64_t) ms5607_prom.sens << SHIFT_SENS) + (((int64_t) ms5607_prom.tcs * dT) >> SHIFT_TCS); */ + /* SENS = ((int64_t) ao_ms5607_prom.sens << SHIFT_SENS) + (((int64_t) ao_ms5607_prom.tcs * dT) >> SHIFT_TCS); */ SENS.high = 0; - SENS.low = (uint32_t) ms5607_prom.sens << SHIFT_SENS; - ao_mul64_32_32(&a, ms5607_prom.tcs, dT); + SENS.low = (uint32_t) ao_ms5607_prom.sens << SHIFT_SENS; + ao_mul64_32_32(&a, ao_ms5607_prom.tcs, dT); ao_rshift64(&a, &a, SHIFT_TCS); ao_plus64(&SENS, &SENS, &a); /**/ diff --git a/src/drivers/ao_pad.c b/src/drivers/ao_pad.c index 62ae68e9..144cbd70 100644 --- a/src/drivers/ao_pad.c +++ b/src/drivers/ao_pad.c @@ -153,11 +153,11 @@ ao_pad_monitor(void) * * v_pyro \ * 100k igniter - * output / + * output / * 100k \ * sense relay - * 27k / - * gnd --- + * 27k / + * gnd --- * * If the relay is closed, then sense will be 0 * If no igniter is present, then sense will be v_pyro * 27k/227k = pyro * 127 / 227 ~= pyro/2 diff --git a/src/drivers/ao_pca9922.c b/src/drivers/ao_pca9922.c index d376b968..6d1b5fae 100644 --- a/src/drivers/ao_pca9922.c +++ b/src/drivers/ao_pca9922.c @@ -66,6 +66,24 @@ ao_led_set_mask(uint8_t colors, uint8_t mask) ao_led_apply(); } +#define LED_TEST 1 +#if LED_TEST +static void +ao_led_test(void) +{ + ao_cmd_hexbyte(); + if (ao_cmd_status != ao_cmd_success) + return; + ao_led_set(ao_cmd_lex_i); + printf("LEDs set to %02x\n", ao_cmd_lex_i); +} + +static const struct ao_cmds ao_led_cmds[] = { + { ao_led_test, "l <value>\0Set LEDs to <value>" }, + { 0, NULL } +}; +#endif + void ao_led_toggle(uint8_t colors) { @@ -86,4 +104,7 @@ ao_led_init(uint8_t enable) { (void) enable; ao_enable_output(AO_PCA9922_CS_PORT, AO_PCA9922_CS_PIN, AO_PCA9922_CS, 1); +#if LED_TEST + ao_cmd_register(&ao_led_cmds[0]); +#endif } diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index d07488d0..0cdcc9fb 100644 --- a/src/drivers/ao_quadrature.c +++ b/src/drivers/ao_quadrature.c @@ -22,11 +22,14 @@ #include <ao_event.h> __xdata int32_t ao_quadrature_count[AO_QUADRATURE_COUNT]; -static uint8_t ao_quadrature_state[AO_QUADRATURE_COUNT]; -static int8_t ao_quadrature_raw[AO_QUADRATURE_COUNT]; +static uint8_t ao_quadrature_state[AO_QUADRATURE_COUNT]; -#define BIT(a,b) ((a) | ((b) << 1)) -#define STATE(old_a, old_b, new_a, new_b) (((BIT(old_a, old_b) << 2) | BIT(new_a, new_b))) +struct ao_debounce { + uint8_t state; + uint8_t count; +}; + +static struct ao_debounce ao_debounce_state[AO_QUADRATURE_COUNT][2]; #define port(q) AO_QUADRATURE_ ## q ## _PORT #define bita(q) AO_QUADRATURE_ ## q ## _A @@ -35,14 +38,35 @@ static int8_t ao_quadrature_raw[AO_QUADRATURE_COUNT]; #define pinb(q) AO_QUADRATURE_ ## q ## _B ## _PIN #define isr(q) ao_quadrature_isr_ ## q -static inline uint16_t -ao_quadrature_read(struct stm_gpio *gpio, uint8_t pin_a, uint8_t pin_b) { - uint16_t v = stm_gpio_get_all(gpio); +#define DEBOUNCE 10 - return ~((((v >> pin_a) & 1) | (((v >> pin_b) & 1) << 1))) & 3; +static uint8_t +ao_debounce(uint8_t cur, struct ao_debounce *debounce) +{ + if (cur == debounce->state) + debounce->count = 0; + else { + if (++debounce->count == DEBOUNCE) { + debounce->state = cur; + debounce->count = 0; + } + } + return debounce->state; } -#define _ao_quadrature_get(q) ao_quadrature_read(port(q), bita(q), bitb(q)) +static uint16_t +ao_quadrature_read(struct stm_gpio *gpio, uint8_t pin_a, uint8_t pin_b, struct ao_debounce debounce_state[2]) { + uint16_t v = ~stm_gpio_get_all(gpio); + uint8_t a = (v >> pin_a) & 1; + uint8_t b = (v >> pin_b) & 1; + + a = ao_debounce(a, &debounce_state[0]); + b = ao_debounce(b, &debounce_state[1]); + + return a | (b << 1); +} + +#define _ao_quadrature_get(q) ao_quadrature_read(port(q), bita(q), bitb(q), ao_debounce_state[q]) static void _ao_quadrature_queue(uint8_t q, int8_t step) @@ -54,51 +78,28 @@ _ao_quadrature_queue(uint8_t q, int8_t step) ao_wakeup(&ao_quadrature_count[q]); } -static const int8_t step[16] = { - [STATE(0,0,0,0)] = 0, - [STATE(0,0,0,1)] = -1, - [STATE(0,0,1,0)] = 1, - [STATE(0,0,1,1)] = 0, - [STATE(0,1,0,0)] = 1, - [STATE(0,1,1,0)] = 0, - [STATE(0,1,1,1)] = -1, - [STATE(1,0,0,0)] = -1, - [STATE(1,0,0,1)] = 0, - [STATE(1,0,1,0)] = 0, - [STATE(1,0,1,1)] = 1, - [STATE(1,1,0,0)] = 0, - [STATE(1,1,0,1)] = 1, - [STATE(1,1,1,0)] = -1, - [STATE(1,1,1,1)] = 0 -}; - static void -_ao_quadrature_set(uint8_t q, uint8_t value) { - uint8_t v; - - v = ao_quadrature_state[q] & 3; - value = value & 3; - - if (v == value) - return; - - ao_quadrature_state[q] = (v << 2) | value; +_ao_quadrature_set(uint8_t q, uint8_t new) { + uint8_t old = ao_quadrature_state[q]; - ao_quadrature_raw[q] += step[ao_quadrature_state[q]]; - if (value == 0) { - if (ao_quadrature_raw[q] == 4) + if (old != new && new == 0) { + if (old & 2) _ao_quadrature_queue(q, 1); - else if (ao_quadrature_raw[q] == -4) + else if (old & 1) _ao_quadrature_queue(q, -1); - ao_quadrature_raw[q] = 0; } + ao_quadrature_state[q] = new; } static void ao_quadrature_isr(void) { +#if AO_QUADRATURE_COUNT > 0 _ao_quadrature_set(0, _ao_quadrature_get(0)); +#endif +#if AO_QUADRATURE_COUNT > 1 _ao_quadrature_set(1, _ao_quadrature_get(1)); +#endif } int32_t @@ -120,6 +121,8 @@ static void ao_quadrature_test(void) { uint8_t q; + int32_t c; + uint8_t s; ao_cmd_decimal(); q = ao_cmd_lex_i; @@ -127,10 +130,18 @@ ao_quadrature_test(void) ao_cmd_status = ao_cmd_syntax_error; return; } - printf ("count %d state %x raw %d\n", - ao_quadrature_count[q], - ao_quadrature_state[q], - ao_quadrature_raw[q]); + + c = -10000; + s = 0; + while (ao_quadrature_count[q] != 10) { + if (ao_quadrature_count[q] != c || + ao_quadrature_state[q] != s) { + c = ao_quadrature_count[q]; + s = ao_quadrature_state[q]; + printf ("count %3d state %2x\n", c, s); + flush(); + } + } #if 0 for (;;) { int32_t c; |