diff options
author | px4dev <px4@purgatory.org> | 2012-10-23 23:38:45 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-10-23 23:51:13 -0700 |
commit | 2fc10320697ecaa9c4e0c52d4d047424e41e6336 (patch) | |
tree | 4f18f494ab811e29dc55452f92a63fff9d271dda /apps/drivers/stm32 | |
parent | 34f99c7dca1995f8ddd9e8d61c4cbd7289f40e99 (diff) | |
download | px4-firmware-2fc10320697ecaa9c4e0c52d4d047424e41e6336.tar.gz px4-firmware-2fc10320697ecaa9c4e0c52d4d047424e41e6336.tar.bz2 px4-firmware-2fc10320697ecaa9c4e0c52d4d047424e41e6336.zip |
Major formatting/whitespace cleanup
Diffstat (limited to 'apps/drivers/stm32')
-rw-r--r-- | apps/drivers/stm32/drv_hrt.c | 89 | ||||
-rw-r--r-- | apps/drivers/stm32/drv_pwm_servo.c | 115 | ||||
-rw-r--r-- | apps/drivers/stm32/tone_alarm/tone_alarm.cpp | 87 |
3 files changed, 170 insertions, 121 deletions
diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index 11b98fd1b..1ac90b16d 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -30,7 +30,7 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - + /** * @file drv_hrt.c * @@ -41,7 +41,7 @@ * Note that really, this could use systick too, but that's * monopolised by NuttX and stealing it would just be awkward. * - * We don't use the NuttX STM32 driver per se; rather, we + * We don't use the NuttX STM32 driver per se; rather, we * claim the timer and then drive it directly. */ @@ -264,10 +264,10 @@ static void hrt_latency_update(void); /* callout list manipulation */ static void hrt_call_internal(struct hrt_call *entry, - hrt_abstime deadline, - hrt_abstime interval, - hrt_callout callout, - void *arg); + hrt_abstime deadline, + hrt_abstime interval, + hrt_callout callout, + void *arg); static void hrt_call_enter(struct hrt_call *entry); static void hrt_call_reschedule(void); static void hrt_call_invoke(void); @@ -372,39 +372,39 @@ static void hrt_tim_init(void) { /* clock/power on our timer */ - modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT); + modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT); - /* claim our interrupt vector */ - irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr); + /* claim our interrupt vector */ + irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr); - /* disable and configure the timer */ - rCR1 = 0; - rCR2 = 0; - rSMCR = 0; - rDIER = DIER_HRT | DIER_PPM; - rCCER = 0; /* unlock CCMR* registers */ - rCCMR1 = CCMR1_PPM; - rCCMR2 = CCMR2_PPM; - rCCER = CCER_PPM; - rDCR = 0; + /* disable and configure the timer */ + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = DIER_HRT | DIER_PPM; + rCCER = 0; /* unlock CCMR* registers */ + rCCMR1 = CCMR1_PPM; + rCCMR2 = CCMR2_PPM; + rCCER = CCER_PPM; + rDCR = 0; - /* configure the timer to free-run at 1MHz */ - rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */ + /* configure the timer to free-run at 1MHz */ + rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */ - /* run the full span of the counter */ - rARR = 0xffff; + /* run the full span of the counter */ + rARR = 0xffff; - /* set an initial capture a little ways off */ - rCCR_HRT = 1000; + /* set an initial capture a little ways off */ + rCCR_HRT = 1000; - /* generate an update event; reloads the counter, all registers */ - rEGR = GTIM_EGR_UG; + /* generate an update event; reloads the counter, all registers */ + rEGR = GTIM_EGR_UG; - /* enable the timer */ - rCR1 = GTIM_CR1_CEN; + /* enable the timer */ + rCR1 = GTIM_CR1_CEN; - /* enable interrupts */ - up_enable_irq(HRT_TIMER_VECTOR); + /* enable interrupts */ + up_enable_irq(HRT_TIMER_VECTOR); } #ifdef CONFIG_HRT_PPM @@ -412,7 +412,7 @@ hrt_tim_init(void) * Handle the PPM decoder state machine. */ static void -hrt_ppm_decode(uint32_t status) +hrt_ppm_decode(uint32_t status) { uint16_t count = rCCR_PPM; uint16_t width; @@ -428,10 +428,11 @@ hrt_ppm_decode(uint32_t status) ppm.last_edge = count; ppm_edge_history[ppm_edge_next++] = width; + if (ppm_edge_next >= 32) ppm_edge_next = 0; - /* + /* * if this looks like a start pulse, then push the last set of values * and reset the state machine */ @@ -441,6 +442,7 @@ hrt_ppm_decode(uint32_t status) if (ppm.next_channel > 4) { for (i = 0; i < ppm.next_channel && i < PPM_MAX_CHANNELS; i++) ppm_buffer[i] = ppm_temp_buffer[i]; + ppm_decoded_channels = i; ppm_last_valid_decode = hrt_absolute_time(); @@ -461,10 +463,11 @@ hrt_ppm_decode(uint32_t status) return; case ARM: + /* we expect a pulse giving us the first mark */ if (width > PPM_MAX_PULSE_WIDTH) goto error; /* pulse was too long */ - + /* record the mark timing, expect an inactive edge */ ppm.last_mark = count; ppm.phase = INACTIVE; @@ -481,6 +484,7 @@ hrt_ppm_decode(uint32_t status) ppm.last_mark = count; ppm_pulse_history[ppm_pulse_next++] = interval; + if (ppm_pulse_next >= 32) ppm_pulse_next = 0; @@ -493,7 +497,7 @@ hrt_ppm_decode(uint32_t status) ppm_temp_buffer[ppm.next_channel++] = interval; ppm.phase = INACTIVE; - return; + return; } @@ -526,9 +530,11 @@ hrt_tim_isr(int irq, void *context) rSR = ~status; #ifdef CONFIG_HRT_PPM + /* was this a PPM edge? */ if (status & (SR_INT_PPM | SR_OVF_PPM)) hrt_ppm_decode(status); + #endif /* was this a timer tick? */ @@ -640,7 +646,7 @@ hrt_init(void) void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) { - hrt_call_internal(entry, + hrt_call_internal(entry, hrt_absolute_time() + delay, 0, callout, @@ -730,9 +736,11 @@ hrt_call_enter(struct hrt_call *entry) //lldbg("call enter at head, reschedule\n"); /* we changed the next deadline, reschedule the timer event */ hrt_call_reschedule(); + } else { do { next = (struct hrt_call *)sq_next(&call->link); + if ((next == NULL) || (entry->deadline < next->deadline)) { //lldbg("call enter after head\n"); sq_addafter(&call->link, &entry->link, &callout_queue); @@ -755,8 +763,10 @@ hrt_call_invoke(void) hrt_abstime now = hrt_absolute_time(); call = (struct hrt_call *)sq_peek(&callout_queue); + if (call == NULL) break; + if (call->deadline > now) break; @@ -764,7 +774,7 @@ hrt_call_invoke(void) //lldbg("call pop\n"); /* save the intended deadline for periodic calls */ - deadline = call->deadline; + deadline = call->deadline; /* zero the deadline, as the call has occurred */ call->deadline = 0; @@ -804,7 +814,7 @@ hrt_call_reschedule() * we want. * * It is important for accurate timekeeping that the compare - * interrupt fires sufficiently often that the base_time update in + * interrupt fires sufficiently often that the base_time update in * hrt_absolute_time runs at least once per timer period. */ if (next != NULL) { @@ -813,11 +823,13 @@ hrt_call_reschedule() //lldbg("pre-expired\n"); /* set a minimal deadline so that we call ASAP */ deadline = now + HRT_INTERVAL_MIN; + } else if (next->deadline < deadline) { //lldbg("due soon\n"); deadline = next->deadline; } } + //lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff)); /* set the new compare value and remember it for latency tracking */ @@ -837,6 +849,7 @@ hrt_latency_update(void) return; } } + /* catch-all at the end */ latency_counters[index]++; } diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c index 708aa8884..be8934492 100644 --- a/apps/drivers/stm32/drv_pwm_servo.c +++ b/apps/drivers/stm32/drv_pwm_servo.c @@ -197,26 +197,29 @@ pwm_channel_init(unsigned channel) /* configure the channel */ switch (pwm_channels[channel].timer_channel) { - case 1: - rCCMR1(timer) |= (6 << 4); - rCCR1(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 0); - break; - case 2: - rCCMR1(timer) |= (6 << 12); - rCCR2(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 4); - break; - case 3: - rCCMR2(timer) |= (6 << 4); - rCCR3(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 8); - break; - case 4: - rCCMR2(timer) |= (6 << 12); - rCCR4(timer) = pwm_channels[channel].default_value; - rCCER(timer) |= (1 << 12); - break; + case 1: + rCCMR1(timer) |= (6 << 4); + rCCR1(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 0); + break; + + case 2: + rCCMR1(timer) |= (6 << 12); + rCCR2(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 4); + break; + + case 3: + rCCMR2(timer) |= (6 << 4); + rCCR3(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 8); + break; + + case 4: + rCCMR2(timer) |= (6 << 12); + rCCR4(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 12); + break; } } @@ -238,22 +241,28 @@ up_pwm_servo_set(unsigned channel, servo_position_t value) /* configure the channel */ if (value > 0) value--; + switch (pwm_channels[channel].timer_channel) { - case 1: - rCCR1(timer) = value; - break; - case 2: - rCCR2(timer) = value; - break; - case 3: - rCCR3(timer) = value; - break; - case 4: - rCCR4(timer) = value; - break; - default: - return -1; + case 1: + rCCR1(timer) = value; + break; + + case 2: + rCCR2(timer) = value; + break; + + case 3: + rCCR3(timer) = value; + break; + + case 4: + rCCR4(timer) = value; + break; + + default: + return -1; } + return 0; } @@ -275,19 +284,23 @@ up_pwm_servo_get(unsigned channel) /* configure the channel */ switch (pwm_channels[channel].timer_channel) { - case 1: - value = rCCR1(timer); - break; - case 2: - value = rCCR2(timer); - break; - case 3: - value = rCCR3(timer); - break; - case 4: - value = rCCR4(timer); - break; + case 1: + value = rCCR1(timer); + break; + + case 2: + value = rCCR2(timer); + break; + + case 3: + value = rCCR3(timer); + break; + + case 4: + value = rCCR4(timer); + break; } + return value; } @@ -303,9 +316,10 @@ up_pwm_servo_init(uint32_t channel_mask) /* now init channels */ for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { /* don't do init for disabled channels; this leaves the pin configs alone */ - if (((1<<i) & channel_mask) && (pwm_channels[i].gpio != 0)) + if (((1 << i) & channel_mask) && (pwm_channels[i].gpio != 0)) pwm_channel_init(i); } + return OK; } @@ -326,17 +340,18 @@ up_pwm_servo_set_rate(unsigned rate) if (pwm_timers[i].base != 0) pwm_timer_set_rate(i, rate); } + return OK; } void up_pwm_servo_arm(bool armed) { - /* + /* * XXX this is inelgant and in particular will either jam outputs at whatever level * they happen to be at at the time the timers stop or generate runts. - * The right thing is almost certainly to kill auto-reload on the timers so that - * they just stop at the end of their count for disable, and to reset/restart them + * The right thing is almost certainly to kill auto-reload on the timers so that + * they just stop at the end of their count for disable, and to reset/restart them * for enable. */ diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp index 00f225dd5..03cf3ee5d 100644 --- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -34,8 +34,8 @@ /** * Driver for the PX4 audio alarm port, /dev/tone_alarm. * - * The tone_alarm driver supports a set of predefined "alarm" - * patterns and one user-supplied pattern. Patterns are ordered by + * The tone_alarm driver supports a set of predefined "alarm" + * patterns and one user-supplied pattern. Patterns are ordered by * priority, with a higher-priority pattern interrupting any * lower-priority pattern that might be playing. * @@ -244,7 +244,8 @@ const tone_note ToneAlarm::_patterns[_max_pattern][_max_pattern_len] = { {{TONE_NOTE_C7, 100}}, {{TONE_NOTE_D7, 100}}, {{TONE_NOTE_E7, 100}}, - { //This is tetris ;) + { + //This is tetris ;) {TONE_NOTE_C6, 40}, {TONE_NOTE_G5, 20}, {TONE_NOTE_G5S, 20}, @@ -361,6 +362,7 @@ ToneAlarm::init() int ret; ret = CDev::init(); + if (ret != OK) return ret; @@ -368,34 +370,34 @@ ToneAlarm::init() stm32_configgpio(GPIO_TONE_ALARM); /* clock/power on our timer */ - modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); + modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); /* initialise the timer */ - rCR1 = 0; - rCR2 = 0; - rSMCR = 0; - rDIER = 0; - rCCER &= TONE_CCER; /* unlock CCMR* registers */ - rCCMR1 = TONE_CCMR1; - rCCMR2 = TONE_CCMR2; - rCCER = TONE_CCER; - rDCR = 0; - - /* toggle the CC output each time the count passes 1 */ - TONE_rCCR = 1; - - /* - * Configure the timebase to free-run at half max frequency. - * XXX this should be more flexible in order to get a better - * frequency range, but for the F4 with the APB1 timers based - * at 42MHz, this gets us down to ~320Hz or so. - */ - rPSC = 1; - - /* make sure the timer is running */ - rCR1 = GTIM_CR1_CEN; - - debug("ready"); + rCR1 = 0; + rCR2 = 0; + rSMCR = 0; + rDIER = 0; + rCCER &= TONE_CCER; /* unlock CCMR* registers */ + rCCMR1 = TONE_CCMR1; + rCCMR2 = TONE_CCMR2; + rCCER = TONE_CCER; + rDCR = 0; + + /* toggle the CC output each time the count passes 1 */ + TONE_rCCR = 1; + + /* + * Configure the timebase to free-run at half max frequency. + * XXX this should be more flexible in order to get a better + * frequency range, but for the F4 with the APB1 timers based + * at 42MHz, this gets us down to ~320Hz or so. + */ + rPSC = 1; + + /* make sure the timer is running */ + rCR1 = GTIM_CR1_CEN; + + debug("ready"); return OK; } @@ -413,6 +415,7 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) switch (cmd) { case TONE_SET_ALARM: debug("TONE_SET_ALARM %u", arg); + if (new_pattern == 0) { /* cancel any current alarm */ _current_pattern = _pattern_none; @@ -431,10 +434,13 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) /* and start playing it */ next(); + } else { /* current pattern is higher priority than the new pattern, ignore */ } + break; + default: result = -ENOTTY; break; @@ -457,8 +463,10 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len) /* sanity-check the size of the write */ if (len > (_max_pattern_len * sizeof(tone_note))) return -EFBIG; + if ((len % sizeof(tone_note)) || (len == 0)) return -EIO; + if (!check((tone_note *)buffer)) return -EIO; @@ -479,6 +487,7 @@ ToneAlarm::write(file *filp, const char *buffer, size_t len) debug("starting user pattern"); next(); } + irqrestore(flags); return len; @@ -511,18 +520,22 @@ ToneAlarm::next(void) /* find the note to play */ if (_current_pattern == _pattern_user) { np = &_user_pattern[_next_note]; + } else { np = &_patterns[_current_pattern - 1][_next_note]; } /* work out which note is next */ _next_note++; + if (_next_note >= _note_max) { /* hit the end of the pattern, stop */ _current_pattern = _pattern_none; + } else if (np[1].duration == DURATION_END) { /* hit the end of the pattern, stop */ _current_pattern = _pattern_none; + } else if (np[1].duration == DURATION_REPEAT) { /* next note is a repeat, rewind in preparation */ _next_note = 0; @@ -534,11 +547,11 @@ ToneAlarm::next(void) /* set reload based on the pitch */ rARR = _notes[np->pitch]; - /* force an update, reloads the counter and all registers */ - rEGR = GTIM_EGR_UG; + /* force an update, reloads the counter and all registers */ + rEGR = GTIM_EGR_UG; - /* enable the output */ - rCCER |= TONE_CCER; + /* enable the output */ + rCCER |= TONE_CCER; } /* arrange a callback when the note/rest is done */ @@ -554,6 +567,7 @@ ToneAlarm::check(tone_note *pattern) if ((i == 0) && ((pattern[i].duration == DURATION_END) || (pattern[i].duration == DURATION_REPEAT))) return false; + if (pattern[i].duration == DURATION_END) break; @@ -561,6 +575,7 @@ ToneAlarm::check(tone_note *pattern) if (pattern[i].pitch >= _note_max) return false; } + return true; } @@ -592,13 +607,16 @@ play_pattern(unsigned pattern) int fd, ret; fd = open("/dev/tone_alarm", 0); + if (fd < 0) err(1, "/dev/tone_alarm"); warnx("playing pattern %u", pattern); ret = ioctl(fd, TONE_SET_ALARM, pattern); + if (ret != 0) err(1, "TONE_SET_ALARM"); + exit(0); } @@ -615,6 +633,7 @@ tone_alarm_main(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "couldn't allocate the ToneAlarm driver"); + if (g_dev->init() != OK) { delete g_dev; errx(1, "ToneAlarm init failed"); @@ -623,8 +642,10 @@ tone_alarm_main(int argc, char *argv[]) if ((argc > 1) && !strcmp(argv[1], "start")) play_pattern(1); + if ((argc > 1) && !strcmp(argv[1], "stop")) play_pattern(0); + if ((pattern = strtol(argv[1], nullptr, 10)) != 0) play_pattern(pattern); |