changed hmi task into usb_task, qor fix, buttons and rotary encoder driver

This commit is contained in:
Anthony Rabine 2023-08-08 17:37:40 +02:00
parent 0f87f64918
commit d10afb0232
17 changed files with 365 additions and 384 deletions

2
.gitignore vendored
View file

@ -48,3 +48,5 @@ software/.vscode/.cortex-debug.registers.state.json
story-player/build/ story-player/build/
story-editor/tools/build-audio-System_Qt5_15_8-Debug/ story-editor/tools/build-audio-System_Qt5_15_8-Debug/
software/.vscode/.cortex-debug.peripherals.state.json

29
software/.gdbinit Normal file
View file

@ -0,0 +1,29 @@
define armex
printf "EXEC_RETURN (LR):\n",
info registers $lr
if (0x04 == (0x04 & (int)$lr))
printf "Uses MSP 0x%x return.\n", $msp
set $armex_base = (int)$msp
else
printf "Uses PSP 0x%x return.\n", $psp
set $armex_base = (int)$psp
end
printf "xPSR 0x%x\n", *($armex_base+28)
printf "ReturnAddress 0x%x\n", *($armex_base+24)
printf "LR (R14) 0x%x\n", *($armex_base+20)
printf "R12 0x%x\n", *($armex_base+16)
printf "R3 0x%x\n", *($armex_base+12)
printf "R2 0x%x\n", *($armex_base+8)
printf "R1 0x%x\n", *($armex_base+4)
printf "R0 0x%x\n", *($armex_base)
printf "Return instruction:\n"
x/i *($armex_base+24)
printf "LR instruction:\n"
x/i *($armex_base+20)
end
document armex
ARMv7 Exception entry behavior.
xPSR, ReturnAddress, LR (R14), R12, R3, R2, R1, and R0
end

View file

@ -1 +0,0 @@
[{"node":"TIMER","expanded":true,"format":0,"pinned":false},{"node":"TIMER.INTR","expanded":true,"format":0},{"node":"TIMER.INTE","expanded":true,"format":0}]

View file

@ -39,7 +39,7 @@ set(OST_SRCS
system/qor_armv6m.s system/qor_armv6m.s
system/audio_player.c system/audio_player.c
system/vm_task.c system/vm_task.c
system/hmi_task.c system/usb_task.c
system/fs_task.c system/fs_task.c
system/ff/ff.c system/ff/ff.c
system/ff/ffsystem.c system/ff/ffsystem.c

View file

@ -31,7 +31,7 @@ add_library(
) )
pico_generate_pio_header(${PROJECT_NAME} ${CMAKE_CURRENT_LIST_DIR}/pico_i2s.pio) pico_generate_pio_header(${PROJECT_NAME} ${CMAKE_CURRENT_LIST_DIR}/pico_i2s.pio)
pico_generate_pio_header(${PROJECT_NAME} ${CMAKE_CURRENT_LIST_DIR}/i2s.pio) pico_generate_pio_header(${PROJECT_NAME} ${CMAKE_CURRENT_LIST_DIR}/pio_rotary_encoder.pio)
target_link_libraries(${PROJECT_NAME} INTERFACE pico_stdlib hardware_exception cmsis_core tinyusb_device tinyusb_board) target_link_libraries(${PROJECT_NAME} INTERFACE pico_stdlib hardware_exception cmsis_core tinyusb_device tinyusb_board)
target_include_directories(${PROJECT_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR}) target_include_directories(${PROJECT_NAME} INTERFACE ${CMAKE_CURRENT_LIST_DIR})

View file

@ -1,253 +0,0 @@
; i2s.pio
;
; Author: Daniel Collins
; Date: 2022-02-25
;
; Copyright (c) 2022 Daniel Collins
;
; This file is part of rp2040_i2s_example.
;
; rp2040_i2s_example is free software: you can redistribute it and/or modify it under
; the terms of the GNU General Public License, version 3 as published by the
; Free Software Foundation.
;
; rp2040_i2s_example is distributed in the hope that it will be useful, but WITHOUT ANY
; WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
; A PARTICULAR PURPOSE. See the GNU General Public License for more details.
;
; You should have received a copy of the GNU General Public License along with
; rp2040_i2s_example. If not, see <https://www.gnu.org/licenses/>.
; An I2S bi-directional peripheral with master clock (SCK) output.
.program i2s_sck
; Transmit an I2S system / master clock.
;
; Usually this needs to be 256 * fs (the word clock rate).
; (e.g. for 48kHz audio, this should be precisely 12.288000 MHz.)
; Since all of these transmit at 1 I2S-clock per 2 sys-clock cycles,
; you need to set the clock divider at 2x the desired rate
; (for the 48kHz example, this should be 2 * 12,288,000 = 24.5760000 MHz).
;
; Use this as one state machine of the I2S PIO unit as a whole for perfect
; synchronization, e.g.:
; state machine 0 = clock (divide to 48000 * 256 * 2 = 24.576000 MHz for 48kHz stereo audio)
; state machine 1 = out (divide to 48000 * 64 * 2 = 6.144000 MHz for 48kHz stereo audio)
; state machine 2 = in (divide to 48000 * 256 * 2 = 24.576000 MHz for 48kHz stereo audio)
;
; One output pin is used for the clock output.
.wrap_target
set pins 1
set pins 0
.wrap
.program i2s_out_master
; I2S audio output block. Synchronous with clock and input.
; Must run at BCK * 2.
;
; This block also outputs the word clock (also called frame or LR clock) and
; the bit clock.
;
; Set register x to (bit depth - 2) (e.g. for 24 bit audio, set to 22).
; Note that if this is needed to be synchronous with the SCK module,
; it is not possible to run 24-bit frames with an SCK of 256x fs. You must either
; run SCK at 384x fs (if your codec permits this) or use 32-bit frames, which
; work fine with 24-bit codecs.
.side_set 2
public entry_point:
; /--- LRCLK
; |/-- BCLK
frameL: ; ||
set x, 30 side 0b00 ; start of Left frame
pull noblock side 0b01 ; One clock after edge change with no data
dataL:
out pins, 1 side 0b00
jmp x-- dataL side 0b01
frameR:
set x, 30 side 0b10
pull noblock side 0b11 ; One clock after edge change with no data
dataR:
out pins, 1 side 0b10
jmp x-- dataR side 0b11
.program i2s_bidi_slave
; I2S audio bidirectional (input/output) block, both in slave mode.
; Requires external BCK and LRCK, usually from the codec directly.
; This block provides both the output and input components together,
; so should not be used in combination with either of the other output or
; input units on the same I2S bus.
;
; Input pin order: DIN, BCK, LRCK
; Set JMP pin to LRCK.
;
; Clock synchronously with the system clock, or *at least* 4x the usual
; bit clock for a given fs (e.g. for 48kHz 24-bit, clock at at least
; (48000 * 24 * 2 (stereo)) * 4 = 9.216 MHz. Ideally 8x or more.
start_l:
wait 1 pin 1 ; DIN should be sampled on rising transition of BCK
in pins, 1 ; first "bit" of new frame is actually LSB of last frame, per I2S
pull noblock ;
push noblock ;
wait 0 pin 1 ; ignore first BCK transition after edge
public_entry_point:
wait 0 pin 2 ; wait for L frame to come around before we start
out pins 1 ; update DOUT on falling BCK edge
loop_l:
wait 1 pin 1 ; DIN should be sampled on rising transition of BCK
in pins, 1 ; read DIN
wait 0 pin 1 ; DOUT should be updated on falling transition of BCK
out pins 1 ; update DOUT
jmp pin start_r ; if LRCK has gone high, we're "done" with this word (1 bit left to read)
jmp loop_l ;
start_r:
wait 1 pin 1 ; wait for the last bit of the previous frame
in pins, 1 ; first "bit" of new frame is actually LSB of last frame, per I2S
pull noblock ; pull next output word from FIFO
push noblock ; push the completed word to the FIFO
wait 0 pin 1 ; wait for next clock cycle
out pins 1 ; update DOUT on falling edge
loop_r:
wait 1 pin 1 ;
in pins 1 ;
wait 0 pin 1 ;
out pins 1 ; update DOUT
jmp pin loop_r ; if LRCK is still high, we're still sampling this word
; implicit jmp to start_l: otherwise, start the loop over
; I2S Audio Input - Slave or Synchronous with Output Master
; Inputs must be sequential in order: DIN, BCK, LRCK
; Must run at same speed of SCK block, or at least 4x BCK.
;
; NOTE: Set JMP pin to LRCK pin.
; NOTE: The very first word read is potentially corrupt, since there is a
; chance to start in the middle of an L frame and only read part of it.
; Nevertheless, the frame order should be synchronized (first word is L,
; second is R, etc.)
.program i2s_in_slave
start_sample_l:
wait 1 pin 1 ; DIN should be sampled on rising transition of BCK
in pins, 1 ; first "bit" of new frame is actually LSB of last frame, per I2S
push noblock ; push however many bits we read into the FIFO
wait 0 pin 1 ; ignore first BCK transition after edge
public_entry_point:
wait 0 pin 2 ; wait for L frame to come around before we start
sample_l:
wait 1 pin 1 ; DIN should be sampled on rising transition of BCK
in pins, 1 ; read DIN
wait 0 pin 1 ; don't sample more than once per rising edge, wait for next clock
jmp pin start_sample_r ; if LRCK has gone high, we're "done" with this word (1 bit left to read)
jmp sample_l
start_sample_r:
wait 1 pin 1 ; wait for the last bit of the previous frame
in pins, 1 ; first "bit" of new frame is actually LSB of last frame, per I2S
push noblock ; push the completed word to the FIFO
wait 0 pin 1 ; wait for next clock cycle
sample_r:
wait 1 pin 1
in pins, 1
wait 0 pin 1
jmp pin sample_r ; if LRCK is still high, we're still sampling this word
; implicit jmp to start_sample_l: otherwise, start the loop over
% c-sdk {
// These constants are the I2S clock to pio clock ratio
const int i2s_sck_program_pio_mult = 2;
const int i2s_out_master_program_pio_mult = 2;
/*
* System ClocK (SCK) is only required by some I2S peripherals.
* This outputs it at 1 SCK per 2 PIO clocks, so scale the dividers correctly
* first.
* NOTE: Most peripherals require that this is *perfectly* aligned in ratio,
* if not phase, to the bit and word clocks of any master peripherals.
* It is up to you to ensure that the divider config is set up for a
* precise (not approximate) ratio between the BCK, LRCK, and SCK outputs.
*/
static void i2s_sck_program_init(PIO pio, uint8_t sm, uint8_t offset, uint8_t sck_pin) {
pio_gpio_init(pio, sck_pin);
pio_sm_config sm_config = i2s_sck_program_get_default_config(offset);
sm_config_set_set_pins(&sm_config, sck_pin, 1);
uint pin_mask = (1u << sck_pin);
pio_sm_set_pins_with_mask(pio, sm, 0, pin_mask); // zero output
pio_sm_set_pindirs_with_mask(pio, sm, pin_mask, pin_mask);
pio_sm_init(pio, sm, offset, &sm_config);
}
static inline void i2s_out_master_program_init(PIO pio, uint8_t sm, uint8_t offset, uint8_t bit_depth, uint8_t dout_pin, uint8_t clock_pin_base) {
pio_gpio_init(pio, dout_pin);
pio_gpio_init(pio, clock_pin_base);
pio_gpio_init(pio, clock_pin_base + 1);
pio_sm_config sm_config = i2s_out_master_program_get_default_config(offset);
sm_config_set_out_pins(&sm_config, dout_pin, 1);
sm_config_set_sideset_pins(&sm_config, clock_pin_base);
sm_config_set_out_shift(&sm_config, false, false, bit_depth);
sm_config_set_fifo_join(&sm_config, PIO_FIFO_JOIN_TX);
pio_sm_init(pio, sm, offset, &sm_config);
uint32_t pin_mask = (1u << dout_pin) | (3u << clock_pin_base);
pio_sm_set_pins_with_mask(pio, sm, 0, pin_mask); // zero output
pio_sm_set_pindirs_with_mask(pio, sm, pin_mask, pin_mask);
}
static inline void i2s_bidi_slave_program_init(PIO pio, uint8_t sm, uint8_t offset, uint8_t dout_pin, uint8_t in_pin_base) {
pio_gpio_init(pio, dout_pin);
pio_gpio_init(pio, in_pin_base);
pio_gpio_init(pio, in_pin_base + 1);
pio_gpio_init(pio, in_pin_base + 2);
pio_sm_config sm_config = i2s_bidi_slave_program_get_default_config(offset);
sm_config_set_out_pins(&sm_config, dout_pin, 1);
sm_config_set_in_pins(&sm_config, in_pin_base);
sm_config_set_jmp_pin(&sm_config, in_pin_base + 2);
sm_config_set_out_shift(&sm_config, false, false, 0);
sm_config_set_in_shift(&sm_config, false, false, 0);
pio_sm_init(pio, sm, offset, &sm_config);
// Setup output pins
uint32_t pin_mask = (1u << dout_pin);
pio_sm_set_pins_with_mask(pio, sm, 0, pin_mask); // zero output
pio_sm_set_pindirs_with_mask(pio, sm, pin_mask, pin_mask);
// Setup input pins
pin_mask = (7u << in_pin_base); // Three input pins
pio_sm_set_pindirs_with_mask(pio, sm, 0, pin_mask);
}
/*
* Designed to be used with output master module, requiring overlapping pins:
* din_pin_base + 0 = input pin
* din_pin_base + 1 = out_master clock_pin_base
* din_pin_base + 2 = out_master clock_pin_base + 1
*
* Intended to be run at SCK rate (4x BCK), so clock same as SCK module if using
* it, or 4x the BCK frequency (BCK is 64x fs, so 256x fs).
*/
static inline void i2s_in_slave_program_init(PIO pio, uint8_t sm, uint8_t offset, uint8_t din_pin_base) {
pio_gpio_init(pio, din_pin_base);
gpio_set_pulls(din_pin_base, false, false);
gpio_set_dir(din_pin_base, GPIO_IN);
pio_sm_config sm_config = i2s_in_slave_program_get_default_config(offset);
sm_config_set_in_pins(&sm_config, din_pin_base);
sm_config_set_in_shift(&sm_config, false, false, 0);
sm_config_set_fifo_join(&sm_config, PIO_FIFO_JOIN_RX);
sm_config_set_jmp_pin(&sm_config, din_pin_base + 2);
pio_sm_init(pio, sm, offset, &sm_config);
uint32_t pin_mask = (7u << din_pin_base); // Three input pins
pio_sm_set_pindirs_with_mask(pio, sm, 0, pin_mask);
}
%}

View file

@ -26,6 +26,7 @@
#include "pico_lcd_spi.h" #include "pico_lcd_spi.h"
#include "audio_player.h" #include "audio_player.h"
#include "pico_i2s.h" #include "pico_i2s.h"
#include "pio_rotary_encoder.pio.h"
// =========================================================================================================== // ===========================================================================================================
// CONSTANTS / DEFINES // CONSTANTS / DEFINES
@ -56,13 +57,31 @@ const uint8_t SD_CARD_PRESENCE = 24;
#define UART_TX_PIN 0 #define UART_TX_PIN 0
#define UART_RX_PIN 1 #define UART_RX_PIN 1
// PICO alarm (RTOS uses Alarm 0 and IRQ 0)
#define ALARM_NUM 1
#define ALARM_IRQ TIMER_IRQ_1
// =========================================================================================================== // ===========================================================================================================
// GLOBAL VARIABLES // GLOBAL VARIABLES
// =========================================================================================================== // ===========================================================================================================
static __attribute__((aligned(8))) pio_i2s i2s; static __attribute__((aligned(8))) pio_i2s i2s;
static volatile uint32_t msTicks = 0; static volatile uint32_t msTicks = 0;
static audio_ctx_t audio_ctx; static audio_ctx_t audio_ctx;
// Buttons
static ost_button_callback_t ButtonCallback = NULL; static ost_button_callback_t ButtonCallback = NULL;
static uint32_t DebounceTs = 0;
static bool IsDebouncing = false;
static uint32_t ButtonsState = 0;
static uint32_t ButtonsStatePrev = 0;
// Rotary encoder
// pio 0 is used
PIO pio = pio1;
// state machine 0
uint8_t sm = 0;
static int new_value, delta, old_value = 0;
// =========================================================================================================== // ===========================================================================================================
// PROTOTYPES // PROTOTYPES
@ -74,46 +93,94 @@ void __isr __time_critical_func(audio_i2s_dma_irq_handler)();
// =========================================================================================================== // ===========================================================================================================
// OST HAL IMPLEMENTATION // OST HAL IMPLEMENTATION
// =========================================================================================================== // ===========================================================================================================
void ost_system_delay_ms(uint32_t delay) void ost_system_delay_ms(uint32_t delay)
{ {
busy_wait_ms(delay); busy_wait_ms(delay);
} }
static uint32_t DebounceTs = 0; void check_buttons()
static bool IsDebouncing = false;
static uint32_t ButtonsState = 0;
static uint32_t ButtonsStatePrev = 0;
void gpio_callback(uint gpio, uint32_t events)
{ {
static bool one_time = true; if (!gpio_get(ROTARY_BUTTON))
if (events & GPIO_IRQ_EDGE_FALL)
{
DebounceTs = time_us_32() / 1000;
IsDebouncing = true;
}
else if (IsDebouncing)
{
IsDebouncing = false;
uint32_t current = time_us_32() / 1000;
if (current - DebounceTs > 50)
{
if (gpio_get(ROTARY_BUTTON))
{ {
ButtonsState |= OST_BUTTON_OK; ButtonsState |= OST_BUTTON_OK;
} }
else
{
ButtonsState &= ~OST_BUTTON_OK;
}
if ((ButtonsStatePrev != ButtonsState) && (ButtonCallback != NULL)) // note: thanks to two's complement arithmetic delta will always
// be correct even when new_value wraps around MAXINT / MININT
new_value = quadrature_encoder_get_count(pio, sm);
delta = new_value - old_value;
old_value = new_value;
if (delta > 0)
{
ButtonsState |= OST_BUTTON_LEFT;
}
else if (delta < 0)
{
ButtonsState |= OST_BUTTON_RIGHT;
}
else
{
ButtonsState &= ~OST_BUTTON_LEFT;
ButtonsState &= ~OST_BUTTON_RIGHT;
}
if (IsDebouncing)
{
// Même état pendant X millisecondes, on valide
if ((ButtonsStatePrev == ButtonsState) && (ButtonCallback != NULL))
{
if (ButtonsState != 0)
{ {
ButtonCallback(ButtonsState); ButtonCallback(ButtonsState);
} }
}
IsDebouncing = false;
ButtonsStatePrev = ButtonsState; ButtonsStatePrev = ButtonsState;
} }
else
{
// Changement d'état détecté
if (ButtonsStatePrev != ButtonsState)
{
ButtonsStatePrev = ButtonsState;
IsDebouncing = true;
} }
} }
}
static void alarm_in_us(uint32_t delay_us);
static void alarm_irq(void)
{
// Clear the alarm irq
hw_clear_bits(&timer_hw->intr, 1u << ALARM_NUM);
alarm_in_us(10000);
check_buttons();
}
static void alarm_in_us(uint32_t delay_us)
{
// Enable the interrupt for our alarm (the timer outputs 4 alarm irqs)
hw_set_bits(&timer_hw->inte, 1u << ALARM_NUM);
// Enable interrupt in block and at processor
// Alarm is only 32 bits so if trying to delay more
// than that need to be careful and keep track of the upper
// bits
uint64_t target = timer_hw->timerawl + delay_us;
// Write the lower 32 bits of the target time to the alarm which
// will arm it
timer_hw->alarm[ALARM_NUM] = (uint32_t)target;
}
void ost_system_initialize() void ost_system_initialize()
{ {
@ -160,17 +227,22 @@ void ost_system_initialize()
ST7789_Fill_Color(MAGENTA); ST7789_Fill_Color(MAGENTA);
//------------------- Rotary encoder init //------------------- Rotary encoder init
gpio_init(ROTARY_A);
gpio_set_dir(ROTARY_A, GPIO_IN);
gpio_init(ROTARY_B);
gpio_set_dir(ROTARY_B, GPIO_IN);
gpio_init(ROTARY_BUTTON); gpio_init(ROTARY_BUTTON);
gpio_set_dir(ROTARY_BUTTON, GPIO_IN); gpio_set_dir(ROTARY_BUTTON, GPIO_IN);
gpio_disable_pulls(ROTARY_BUTTON); gpio_disable_pulls(ROTARY_BUTTON);
gpio_set_irq_enabled_with_callback(ROTARY_BUTTON, GPIO_IRQ_EDGE_FALL | GPIO_IRQ_EDGE_RISE, true, &gpio_callback); // Rotary Encoder is managed by the PIO !
// we don't really need to keep the offset, as this program must be loaded
// at offset 0
pio_add_program(pio, &quadrature_encoder_program);
quadrature_encoder_program_init(pio, sm, ROTARY_A, 0);
// Set irq handler for alarm irq
irq_set_exclusive_handler(ALARM_IRQ, alarm_irq);
// Enable the alarm irq
irq_set_enabled(ALARM_IRQ, true);
alarm_in_us(100000);
//------------------- Init SDCARD //------------------- Init SDCARD
gpio_init(SD_CARD_CS); gpio_init(SD_CARD_CS);

View file

@ -1,25 +1,5 @@
; i2s.pio ; i2s.pio
; ;
; Author: Daniel Collins
; Date: 2022-02-25
;
; Copyright (c) 2022 Daniel Collins
;
; This file is part of rp2040_i2s_example.
;
; rp2040_i2s_example is free software: you can redistribute it and/or modify it under
; the terms of the GNU General Public License, version 3 as published by the
; Free Software Foundation.
;
; rp2040_i2s_example is distributed in the hope that it will be useful, but WITHOUT ANY
; WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
; A PARTICULAR PURPOSE. See the GNU General Public License for more details.
;
; You should have received a copy of the GNU General Public License along with
; rp2040_i2s_example. If not, see <https://www.gnu.org/licenses/>.
; An I2S bi-directional peripheral with master clock (SCK) output.
.program i2s_out_master .program i2s_out_master
; I2S audio output block. Synchronous with clock and input. ; I2S audio output block. Synchronous with clock and input.

View file

@ -0,0 +1,141 @@
;
; Copyright (c) 2023 Raspberry Pi (Trading) Ltd.
;
; SPDX-License-Identifier: BSD-3-Clause
;
.program quadrature_encoder
; the code must be loaded at address 0, because it uses computed jumps
.origin 0
; the code works by running a loop that continuously shifts the 2 phase pins into
; ISR and looks at the lower 4 bits to do a computed jump to an instruction that
; does the proper "do nothing" | "increment" | "decrement" action for that pin
; state change (or no change)
; ISR holds the last state of the 2 pins during most of the code. The Y register
; keeps the current encoder count and is incremented / decremented according to
; the steps sampled
; the program keeps trying to write the current count to the RX FIFO without
; blocking. To read the current count, the user code must drain the FIFO first
; and wait for a fresh sample (takes ~4 SM cycles on average). The worst case
; sampling loop takes 10 cycles, so this program is able to read step rates up
; to sysclk / 10 (e.g., sysclk 125MHz, max step rate = 12.5 Msteps/sec)
; 00 state
JMP update ; read 00
JMP decrement ; read 01
JMP increment ; read 10
JMP update ; read 11
; 01 state
JMP increment ; read 00
JMP update ; read 01
JMP update ; read 10
JMP decrement ; read 11
; 10 state
JMP decrement ; read 00
JMP update ; read 01
JMP update ; read 10
JMP increment ; read 11
; to reduce code size, the last 2 states are implemented in place and become the
; target for the other jumps
; 11 state
JMP update ; read 00
JMP increment ; read 01
decrement:
; note: the target of this instruction must be the next address, so that
; the effect of the instruction does not depend on the value of Y. The
; same is true for the "JMP X--" below. Basically "JMP Y--, <next addr>"
; is just a pure "decrement Y" instruction, with no other side effects
JMP Y--, update ; read 10
; this is where the main loop starts
.wrap_target
update:
MOV ISR, Y ; read 11
PUSH noblock
sample_pins:
; we shift into ISR the last state of the 2 input pins (now in OSR) and
; the new state of the 2 pins, thus producing the 4 bit target for the
; computed jump into the correct action for this state. Both the PUSH
; above and the OUT below zero out the other bits in ISR
OUT ISR, 2
IN PINS, 2
; save the state in the OSR, so that we can use ISR for other purposes
MOV OSR, ISR
; jump to the correct state machine action
MOV PC, ISR
; the PIO does not have a increment instruction, so to do that we do a
; negate, decrement, negate sequence
increment:
MOV Y, ~Y
JMP Y--, increment_cont
increment_cont:
MOV Y, ~Y
.wrap ; the .wrap here avoids one jump instruction and saves a cycle too
% c-sdk {
#include "hardware/clocks.h"
#include "hardware/gpio.h"
// max_step_rate is used to lower the clock of the state machine to save power
// if the application doesn't require a very high sampling rate. Passing zero
// will set the clock to the maximum
static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint pin, int max_step_rate)
{
pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false);
// gpio_pull_up(pin);
// gpio_pull_up(pin + 1);
pio_sm_config c = quadrature_encoder_program_get_default_config(0);
sm_config_set_in_pins(&c, pin); // for WAIT, IN
sm_config_set_jmp_pin(&c, pin); // for JMP
// shift to left, autopull disabled
sm_config_set_in_shift(&c, false, false, 32);
// don't join FIFO's
sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE);
// passing "0" as the sample frequency,
if (max_step_rate == 0) {
sm_config_set_clkdiv(&c, 1.0);
} else {
// one state machine loop takes at most 10 cycles
float div = (float)clock_get_hz(clk_sys) / (10 * max_step_rate);
sm_config_set_clkdiv(&c, div);
}
pio_sm_init(pio, sm, 0, &c);
pio_sm_set_enabled(pio, sm, true);
}
static inline int32_t quadrature_encoder_get_count(PIO pio, uint sm)
{
uint ret;
int n;
// if the FIFO has N entries, we fetch them to drain the FIFO,
// plus one entry which will be guaranteed to not be stale
n = pio_sm_get_rx_fifo_level(pio, sm) + 1;
while (n > 0) {
ret = pio_sm_get_blocking(pio, sm);
n--;
}
return ret;
}
%}

View file

@ -1,10 +0,0 @@
#ifndef HMI_TASK_H
#define HMI_TASK_H
#include <stdint.h>
void hmi_task_initialize();
void hmi_task_ost_ready(uint32_t number_of_stories);
#endif // HMI_TASK_H

View file

@ -14,7 +14,7 @@
#include "chip32_vm.h" #include "chip32_vm.h"
#include "system.h" #include "system.h"
#include "hmi_task.h" #include "usb_task.h"
#include "vm_task.h" #include "vm_task.h"
#include "fs_task.h" #include "fs_task.h"
@ -32,7 +32,7 @@ void IdleTask(void *args)
while (1) while (1)
{ {
// Instrumentation, power saving, os functions won't work here // Instrumentation, power saving, os functions won't work here
// __asm volatile("wfi"); __asm volatile("wfi");
} }
} }
@ -54,7 +54,7 @@ int main()
qor_init(125000000UL); qor_init(125000000UL);
// 5. Initialize the tasks // 5. Initialize the tasks
hmi_task_initialize(); usb_task_initialize();
vm_task_initialize(); vm_task_initialize();
fs_task_initialize(); fs_task_initialize();

View file

@ -40,7 +40,9 @@ extern "C"
OST_BUTTON_HOME = 0x02, OST_BUTTON_HOME = 0x02,
OST_BUTTON_PAUSE = 0x04, OST_BUTTON_PAUSE = 0x04,
OST_BUTTON_VOLUME_UP = 0x08, OST_BUTTON_VOLUME_UP = 0x08,
OST_BUTTON_VOLUME_DOWN = 0x10 OST_BUTTON_VOLUME_DOWN = 0x10,
OST_BUTTON_LEFT = 0x20,
OST_BUTTON_RIGHT = 0x40
} ost_button_t; } ost_button_t;
// ---------------------------------------------------------------------------- // ----------------------------------------------------------------------------

View file

@ -1,7 +1,7 @@
#ifndef QOR_H #ifndef QOR_H
#define QOR_H #define QOR_H
#define QOR_VERSION "0.1" #define QOR_VERSION "0.2"
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>

View file

@ -40,7 +40,7 @@ qor_go:
mov lr, r5 @ Update LR mov lr, r5 @ Update LR
pop {r3} @ Task code pop {r3} @ Task code
pop {r2} @ Pop and discard XPSR. */ pop {r2} @ Pop and discard XPSR. */
cpsie i @ Enable interrupts before launching the cpsie i @ Enable interrupts
bx r3 @ /* Finally, jump to the user defined task code. */ bx r3 @ /* Finally, jump to the user defined task code. */

View file

@ -43,14 +43,14 @@ typedef enum
// GLOBAL STORY VARIABLES // GLOBAL STORY VARIABLES
// =========================================================================================================== // ===========================================================================================================
static qor_tcb_t HmiTcb; static qor_tcb_t UsbTcb;
static uint32_t HmiStack[4096]; static uint32_t UsbStack[4096];
static qor_mbox_t HmiMailBox; static qor_mbox_t UsbMailBox;
static ost_hmi_event_t HmiEvent; static ost_hmi_event_t UsbEvent;
static ost_hmi_event_t *HmiQueue[10]; static ost_hmi_event_t *UsbQueue[10];
static ost_system_state_t OstState = OST_SYS_WAIT_INDEX; static ost_system_state_t OstState = OST_SYS_WAIT_INDEX;
@ -59,7 +59,7 @@ static ost_context_t OstContext;
// =========================================================================================================== // ===========================================================================================================
// HMI TASK (user interface, buttons manager, LCD) // HMI TASK (user interface, buttons manager, LCD)
// =========================================================================================================== // ===========================================================================================================
void HmiTask(void *args) void UsbTask(void *args)
{ {
ost_hmi_event_t *e = NULL; ost_hmi_event_t *e = NULL;
@ -76,14 +76,14 @@ void HmiTask(void *args)
#include "msc_disk.h" #include "msc_disk.h"
void hmi_task_initialize() void usb_task_initialize()
{ {
OstState = OST_SYS_WAIT_INDEX; OstState = OST_SYS_WAIT_INDEX;
qor_mbox_init(&HmiMailBox, (void **)&HmiQueue, 10); qor_mbox_init(&UsbMailBox, (void **)&UsbQueue, 10);
msc_disk_initialize(); msc_disk_initialize();
qor_create_thread(&HmiTcb, HmiTask, HmiStack, sizeof(HmiStack) / sizeof(HmiStack[0]), HMI_TASK_PRIORITY, "HmiTask"); // less priority is the HMI (user inputs and LCD) qor_create_thread(&UsbTcb, UsbTask, UsbStack, sizeof(UsbStack) / sizeof(UsbStack[0]), HMI_TASK_PRIORITY, "UsbTask"); // less priority is the HMI (user inputs and LCD)
} }
//--------------------------------------------------------------------+ //--------------------------------------------------------------------+

View file

@ -0,0 +1,8 @@
#ifndef USB_TASK_H
#define USB_TASK_H
#include <stdint.h>
void usb_task_initialize();
#endif // USB_TASK_H

View file

@ -27,15 +27,15 @@ typedef enum
} ost_vm_ev_type_t; } ost_vm_ev_type_t;
typedef struct typedef struct
{ {
uint32_t button_mask;
ost_vm_ev_type_t ev; ost_vm_ev_type_t ev;
const char *story_dir; const char *story_dir;
uint32_t button_mask;
} ost_vm_event_t; } ost_vm_event_t;
typedef enum typedef enum
{ {
OST_VM_STATE_HOME, OST_VM_STATE_HOME,
OST_VM_STATE_HOME_WAIT_FS, OST_VM_STATE_HOME_WAIT_LOAD_STORY,
OST_VM_STATE_RUN_STORY, OST_VM_STATE_RUN_STORY,
OST_VM_STATE_WAIT_BUTTON, OST_VM_STATE_WAIT_BUTTON,
OST_VM_STATE_ERROR //!< General error, cannot continue OST_VM_STATE_ERROR //!< General error, cannot continue
@ -160,11 +160,10 @@ void VmTask(void *args)
chip32_result_t run_result; chip32_result_t run_result;
ost_vm_event_t *message = NULL; ost_vm_event_t *message = NULL;
uint32_t res = 0; uint32_t res = 0;
bool isHome = true;
ost_vm_state_t VmState = OST_VM_STATE_HOME; ost_vm_state_t VmState = OST_VM_STATE_HOME;
fs_task_scan_index(read_index_callback); fs_task_scan_index(read_index_callback);
isHome = true; bool run_script = false;
while (1) while (1)
{ {
@ -181,11 +180,72 @@ void VmTask(void *args)
// La lecture de l'index est terminée, on demande l'affichage des médias // La lecture de l'index est terminée, on demande l'affichage des médias
fs_task_play_index(); fs_task_play_index();
break; break;
case VM_EV_BUTTON_EVENT:
// debug_printf("B: %x", message->button_mask);
if ((message->button_mask & OST_BUTTON_OK) == OST_BUTTON_OK)
{
VmState = OST_VM_STATE_HOME_WAIT_LOAD_STORY;
debug_printf("OK\r\n");
fs_task_load_story(m_rom_data);
}
break;
case VM_EV_ERROR: case VM_EV_ERROR:
default: default:
break; break;
} }
case OST_VM_STATE_HOME_WAIT_LOAD_STORY:
if (message->ev == VM_EV_START_STORY_EVENT)
{
// Launch the execution of a story
chip32_initialize(&m_chip32_ctx);
VmState = OST_VM_STATE_RUN_STORY;
run_script = true;
}
// Pas de break, on enchaîne sur le déroulement du script
case OST_VM_STATE_RUN_STORY:
{
if (message->ev == VM_EV_END_OF_SOUND)
{
run_script = true;
}
if (message->ev == VM_EV_BUTTON_EVENT)
{
if ((message->button_mask & OST_BUTTON_OK) == OST_BUTTON_OK)
{
debug_printf("OK\r\n");
m_chip32_ctx.registers[R0] = 0x01;
run_script = true;
}
else if ((message->button_mask & OST_BUTTON_LEFT) == OST_BUTTON_LEFT)
{
debug_printf("<-\r\n");
m_chip32_ctx.registers[R0] = 0x02;
run_script = true;
}
else if ((message->button_mask & OST_BUTTON_RIGHT) == OST_BUTTON_RIGHT)
{
debug_printf("->\r\n");
m_chip32_ctx.registers[R0] = 0x04;
run_script = true;
}
}
if (run_script)
{
do
{
run_result = chip32_step(&m_chip32_ctx);
} while (run_result == VM_OK);
}
run_script = false;
break;
}
default: default:
break; break;
} }
@ -214,52 +274,3 @@ void vm_task_initialize()
ost_button_register_callback(button_callback); ost_button_register_callback(button_callback);
} }
/*
case OST_VM_STATE_RUN_STORY:
do
{
run_result = chip32_step(&m_chip32_ctx);
} while (run_result == VM_OK);
// pour le moment, dans tous les cas on attend un événement
// Que ce soit par erreur, ou l'attente un appui boutton...
VmState = OST_VM_STATE_WAIT_BUTTON;
break;
case OST_VM_STATE_ERROR:
qor_sleep(20);
break;
case OST_VM_STATE_WAIT_BUTTON:
default:
if (isHome)
{
if (message->ev == VM_EV_BUTTON_EVENT)
{
if ((message->button_mask & OST_BUTTON_OK) == OST_BUTTON_OK)
{
// Load story from disk
debug_printf("Clicked OK\r\n");
fs_task_load_story(m_rom_data);
}
}
else if (message->ev == VM_EV_START_STORY_EVENT)
{
// Launch the execution of a story
chip32_initialize(&m_chip32_ctx);
isHome = false;
VmState = OST_VM_STATE_RUN_STORY;
}
}
else
{
// VM en cours d'exécution
if (message->ev == VM_EV_END_OF_SOUND)
{
VmState = OST_VM_STATE_RUN_STORY;
}
}
}
*/