X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_pad.c;h=120ce5398cfdd4162a187c1c9277bffb667ae43f;hp=21aa788d0e17a9d7b830e1e2548c73d3aae1fd4e;hb=d4b1dffeef3e9ea96e143f74782e4da7d116c0d4;hpb=4d4ad34aec0c75c66162b992f1e52947e4685730 diff --git a/src/drivers/ao_pad.c b/src/drivers/ao_pad.c index 21aa788d..120ce539 100644 --- a/src/drivers/ao_pad.c +++ b/src/drivers/ao_pad.c @@ -18,16 +18,25 @@ #include #include #include +#include -__xdata uint8_t ao_pad_ignite; +static __xdata uint8_t ao_pad_ignite; +static __xdata struct ao_pad_command command; +static __xdata struct ao_pad_query query; +static __pdata uint8_t ao_pad_armed; +static __pdata uint16_t ao_pad_arm_time; +static __pdata uint8_t ao_pad_box; +static __xdata uint8_t ao_pad_disabled; -#define ao_pad_igniter_status(c) AO_PAD_IGNITER_STATUS_UNKNOWN -#define ao_pad_arm_status() AO_PAD_ARM_STATUS_UNKNOWN +#define DEBUG 1 -#if 0 -#define PRINTD(...) printf(__VA_ARGS__) +#if DEBUG +static __pdata uint8_t ao_pad_debug; +#define PRINTD(...) (ao_pad_debug ? (printf(__VA_ARGS__), 0) : 0) +#define FLUSHD() (ao_pad_debug ? (flush(), 0) : 0) #else #define PRINTD(...) +#define FLUSHD() #endif static void @@ -48,48 +57,148 @@ ao_pad_run(void) } } +#define AO_PAD_ARM_BEEP_INTERVAL 200 + static void -ao_pad_status(void) +ao_pad_monitor(void) { + uint8_t c; + uint8_t sample; + __pdata uint8_t prev = 0, cur = 0; + __pdata uint8_t beeping = 0; + __xdata struct ao_data *packet; + __pdata uint16_t arm_beep_time = 0; + + sample = ao_data_head; for (;;) { - ao_delay(AO_SEC_TO_TICKS(1)); -#if 0 - if (ao_igniter_status(ao_igniter_drogue) == ao_igniter_ready) { - if (ao_igniter_status(ao_igniter_main) == ao_igniter_ready) { - for (i = 0; i < 5; i++) { - ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(50)); - ao_delay(AO_MS_TO_TICKS(100)); - } - } else { - ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200)); + __pdata int16_t pyro; + ao_arch_critical( + while (sample == ao_data_head) + ao_sleep((void *) DATA_TO_XDATA(&ao_data_head)); + ); + + packet = &ao_data_ring[sample]; + sample = ao_data_ring_next(sample); + + pyro = packet->adc.pyro; + +#define VOLTS_TO_PYRO(x) ((int16_t) ((x) * 27.0 / 127.0 / 3.3 * 32767.0)) + + cur = 0; + if (pyro > VOLTS_TO_PYRO(10)) { + query.arm_status = AO_PAD_ARM_STATUS_ARMED; + cur |= AO_LED_ARMED; + } else if (pyro < VOLTS_TO_PYRO(5)) { + query.arm_status = AO_PAD_ARM_STATUS_DISARMED; + arm_beep_time = 0; + } else { + if ((ao_time() % 100) < 50) + cur |= AO_LED_ARMED; + query.arm_status = AO_PAD_ARM_STATUS_UNKNOWN; + arm_beep_time = 0; + } + + for (c = 0; c < AO_PAD_NUM; c++) { + int16_t sense = packet->adc.sense[c]; + uint8_t status = AO_PAD_IGNITER_STATUS_UNKNOWN; + + /* + * pyro is run through a divider, so pyro = v_pyro * 27 / 127 ~= v_pyro / 20 + * v_pyro = pyro * 127 / 27 + * + * v_pyro \ + * 100k igniter + * output / + * 100k \ + * sense relay + * 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 + * If igniter is present, then sense will be v_pyro * 27k/127k ~= v_pyro / 20 = pyro + */ + + if (sense <= pyro / 8) { + status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_CLOSED; + if ((ao_time() % 100) < 50) + cur |= AO_LED_CONTINUITY(c); + } + else if (pyro / 8 * 3 <= sense && sense <= pyro / 8 * 5) + status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN; + else if (pyro / 8 * 7 <= sense) { + status = AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN; + cur |= AO_LED_CONTINUITY(c); } + query.igniter_status[c] = status; + } + if (cur != prev) { + PRINTD("change leds from %02x to %02x mask %02x\n", + prev, cur, AO_LED_CONTINUITY_MASK|AO_LED_ARMED); + ao_led_set_mask(cur, AO_LED_CONTINUITY_MASK | AO_LED_ARMED); + prev = cur; + } + + if (ao_pad_armed && (int16_t) (ao_time() - ao_pad_arm_time) > AO_PAD_ARM_TIME) + ao_pad_armed = 0; + + if (ao_pad_armed) { + if (sample & 2) + ao_beep(AO_BEEP_HIGH); + else + ao_beep(AO_BEEP_LOW); + beeping = 1; + } else if (query.arm_status == AO_PAD_ARM_STATUS_ARMED && !beeping) { + if (arm_beep_time == 0) { + arm_beep_time = AO_PAD_ARM_BEEP_INTERVAL; + beeping = 1; + ao_beep(AO_BEEP_HIGH); + } + --arm_beep_time; + } else if (beeping) { + beeping = 0; + ao_beep(0); } -#endif } } -static __pdata uint8_t ao_pad_armed; -static __pdata uint16_t ao_pad_arm_time; -static __pdata uint8_t ao_pad_box; +void +ao_pad_disable(void) +{ + if (!ao_pad_disabled) { + ao_pad_disabled = 1; + ao_radio_recv_abort(); + } +} + +void +ao_pad_enable(void) +{ + ao_pad_disabled = 0; + ao_wakeup (&ao_pad_disabled); +} static void ao_pad(void) { - static __xdata struct ao_pad_command command; - static __xdata struct ao_pad_query query; int16_t time_difference; - uint8_t c; + int8_t ret; - ao_led_off(AO_LED_RED); ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200)); - ao_pad_box = ao_74hc497_read(); + ao_pad_box = 0; + ao_led_set(0); + ao_led_on(AO_LED_POWER); for (;;) { - flush(); - if (ao_radio_cmac_recv(&command, sizeof (command), 0) != AO_RADIO_CMAC_OK) + FLUSHD(); + while (ao_pad_disabled) + ao_sleep(&ao_pad_disabled); + ret = ao_radio_cmac_recv(&command, sizeof (command), 0); + PRINTD ("cmac_recv %d\n", ret); + if (ret != AO_RADIO_CMAC_OK) continue; - PRINTD ("tick %d serial %d cmd %d channel %d\n", - command.tick, command.serial, command.cmd, command.channel); + PRINTD ("tick %d box %d cmd %d channels %02x\n", + command.tick, command.box, command.cmd, command.channels); switch (command.cmd) { case AO_LAUNCH_ARM: @@ -125,12 +234,13 @@ ao_pad(void) query.box = ao_pad_box; query.channels = AO_PAD_ALL_PINS; query.armed = ao_pad_armed; - query.arm_status = ao_pad_arm_status(); - for (c = 0; c < AO_PAD_NUM; c++) - query.igniter_status[c] = ao_pad_igniter_status(c); - PRINTD ("query tick %d serial %d channel %d valid %d arm %d igniter %d\n", - query.tick, query.serial, query.channel, query.valid, query.arm_status, - query.igniter_status); + PRINTD ("query tick %d box %d channels %02x arm %d arm_status %d igniter %d,%d,%d,%d\n", + query.tick, query.box, query.channels, query.armed, + query.arm_status, + query.igniter_status[0], + query.igniter_status[1], + query.igniter_status[2], + query.igniter_status[3]); ao_radio_cmac_send(&query, sizeof (query)); break; case AO_LAUNCH_FIRE: @@ -152,6 +262,7 @@ ao_pad(void) } PRINTD ("ignite\n"); ao_pad_ignite = ao_pad_armed; + ao_pad_arm_time = ao_time(); ao_wakeup(&ao_pad_ignite); break; } @@ -161,27 +272,30 @@ ao_pad(void) void ao_pad_test(void) { -#if 0 - switch (ao_igniter_status(ao_igniter_drogue)) { - case ao_igniter_ready: - case ao_igniter_active: - printf ("Armed: "); - switch (ao_igniter_status(ao_igniter_main)) { - default: - printf("unknown status\n"); - break; - case ao_igniter_ready: - printf("igniter good\n"); - break; - case ao_igniter_open: - printf("igniter bad\n"); - break; - } + uint8_t c; + + printf ("Arm switch: "); + switch (query.arm_status) { + case AO_PAD_ARM_STATUS_ARMED: + printf ("Armed\n"); + break; + case AO_PAD_ARM_STATUS_DISARMED: + printf ("Disarmed\n"); + break; + case AO_PAD_ARM_STATUS_UNKNOWN: + printf ("Unknown\n"); break; - default: - printf("Disarmed\n"); } -#endif + + for (c = 0; c < AO_PAD_NUM; c++) { + printf ("Pad %d: "); + switch (query.igniter_status[c]) { + case AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_CLOSED: printf ("No igniter. Relay closed\n"); break; + case AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN: printf ("No igniter. Relay open\n"); break; + case AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN: printf ("Good igniter. Relay open\n"); break; + case AO_PAD_IGNITER_STATUS_UNKNOWN: printf ("Unknown\n"); break; + } + } } void @@ -190,18 +304,33 @@ ao_pad_manual(void) ao_cmd_white(); if (!ao_match_word("DoIt")) return; - ao_cmd_white(); - ao_pad_ignite = 1; + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + ao_pad_ignite = 1 << ao_cmd_lex_i; ao_wakeup(&ao_pad_ignite); } static __xdata struct ao_task ao_pad_task; static __xdata struct ao_task ao_pad_ignite_task; -static __xdata struct ao_task ao_pad_status_task; +static __xdata struct ao_task ao_pad_monitor_task; + +#if DEBUG +void +ao_pad_set_debug(void) +{ + ao_cmd_decimal(); + if (ao_cmd_status == ao_cmd_success) + ao_pad_debug = ao_cmd_lex_i != 0; +} +#endif __code struct ao_cmds ao_pad_cmds[] = { { ao_pad_test, "t\0Test pad continuity" }, - { ao_pad_manual, "i \0Fire igniter. is doit with D&I" }, + { ao_pad_manual, "i \0Fire igniter. is doit with D&I" }, +#if DEBUG + { ao_pad_set_debug, "D <0 off, 1 on>\0Debug" }, +#endif { 0, NULL } }; @@ -223,5 +352,5 @@ ao_pad_init(void) ao_cmd_register(&ao_pad_cmds[0]); ao_add_task(&ao_pad_task, ao_pad, "pad listener"); ao_add_task(&ao_pad_ignite_task, ao_pad_run, "pad igniter"); - ao_add_task(&ao_pad_status_task, ao_pad_status, "pad status"); + ao_add_task(&ao_pad_monitor_task, ao_pad_monitor, "pad monitor"); }