diff --git a/source/Core/BSP/Magic/BSP.cpp b/source/Core/BSP/Magic/BSP.cpp new file mode 100644 index 00000000..4b06fae3 --- /dev/null +++ b/source/Core/BSP/Magic/BSP.cpp @@ -0,0 +1,92 @@ +// BSP mapping functions + +#include "BSP.h" +#include "I2C_Wrapper.hpp" +#include "IRQ.h" +#include "Pins.h" +#include "Setup.h" +#include "TipThermoModel.h" +#include "configuration.h" +#include "gd32vf103_timer.h" +#include "history.hpp" +#include "main.hpp" + +const uint16_t powerPWM = 255; +const uint8_t holdoffTicks = 10; +const uint8_t tempMeasureTicks = 14; + +uint16_t totalPWM; // Total length of the cycle's ticks + +void resetWatchdog() { fwdgt_counter_reload(); } + +uint16_t getHandleTemperature(uint8_t sample) { +#ifdef TEMP_TMP36 + // We return the current handle temperature in X10 C + // TMP36 in handle, 0.5V offset and then 10mV per deg C (0.75V @ 25C for + // example) STM32 = 4096 count @ 3.3V input -> But We oversample by 32/(2^2) = + // 8 times oversampling Therefore 32768 is the 3.3V input, so 0.1007080078125 + // mV per count So we need to subtract an offset of 0.5V to center on 0C + // (4964.8 counts) + // + int32_t result = getADCHandleTemp(sample); + result -= 4965; // remove 0.5V offset + // 10mV per C + // 99.29 counts per Deg C above 0C + result *= 100; + result /= 993; + return result; +#else +#error Pinecil only uses TMP36 +#endif +} +uint16_t getInputVoltageX10(uint16_t divisor, uint8_t sample) { + uint32_t res = getADCVin(sample); + res *= 4; + res /= divisor; + return res; +} + +void unstick_I2C() { + /* configure SDA/SCL for GPIO */ + GPIO_BC(GPIOB) |= SDA_Pin | SCL_Pin; + gpio_init(SDA_GPIO_Port, GPIO_MODE_OUT_OD, GPIO_OSPEED_50MHZ, SDA_Pin | SCL_Pin); + for (int i = 0; i < 8; i++) { + asm("nop"); + asm("nop"); + asm("nop"); + asm("nop"); + asm("nop"); + GPIO_BOP(GPIOB) |= SCL_Pin; + asm("nop"); + asm("nop"); + asm("nop"); + asm("nop"); + asm("nop"); + GPIO_BOP(GPIOB) &= SCL_Pin; + } + /* connect PB6 to I2C0_SCL */ + /* connect PB7 to I2C0_SDA */ + gpio_init(SDA_GPIO_Port, GPIO_MODE_AF_OD, GPIO_OSPEED_50MHZ, SDA_Pin | SCL_Pin); +} + +uint8_t getButtonA() { return (gpio_input_bit_get(KEY_A_GPIO_Port, KEY_A_Pin) == SET) ? 1 : 0; } +uint8_t getButtonB() { return (gpio_input_bit_get(KEY_B_GPIO_Port, KEY_B_Pin) == SET) ? 1 : 0; } + +void reboot() { + // Spin for watchdog + for (;;) {} +} + +void delay_ms(uint16_t count) { delay_1ms(count); } +uint32_t __get_IPSR(void) { + return 0; // To shut-up CMSIS +} + +bool isTipDisconnected() { + + uint16_t tipDisconnectedThres = TipThermoModel::getTipMaxInC() - 5; + uint32_t tipTemp = TipThermoModel::getTipInC(); + return tipTemp > tipDisconnectedThres; +} + +void setStatusLED(const enum StatusLED state) {} diff --git a/source/Core/BSP/Magic/Debug.cpp b/source/Core/BSP/Magic/Debug.cpp new file mode 100644 index 00000000..c9d5d3bf --- /dev/null +++ b/source/Core/BSP/Magic/Debug.cpp @@ -0,0 +1,61 @@ +/* + * Debug.cpp + * + * Created on: 26 Jan. 2021 + * Author: Ralim + */ +#include "Debug.h" +#include "FreeRTOS.h" +#include "Pins.h" + +extern "C" { + +#include "gd32vf103_usart.h" +} +char uartOutputBuffer[uartOutputBufferLength]; +volatile uint32_t currentOutputPos = 0xFF; +volatile uint32_t outputLength = 0; +extern volatile uint8_t pendingPWM; +void log_system_state(int32_t PWMWattsx10) { + if (currentOutputPos == 0xFF) { + + // Want to print a CSV log out the uart + // Tip_Temp_C,Handle_Temp_C,Output_Power_Wattx10,PWM,Tip_Raw\r\n + // 3+1+3+1+3+1+3+1+5+2 = 23, so sizing at 32 for now + + outputLength = snprintf(uartOutputBuffer, uartOutputBufferLength, "%lu,%u,%li,%u,%lu\r\n", // + TipThermoModel::getTipInC(false), // Tip temp in C + getHandleTemperature(0), // Handle temp in C X10 + PWMWattsx10, // Output Wattage + pendingPWM, // PWM + TipThermoModel::convertTipRawADCTouV(getTipRawTemp(0), true) // Tip temp in uV + ); + + // Now print this out the uart via IRQ (DMA cant be used as oled has it) + currentOutputPos = 0; + /* enable USART1 Transmit Buffer Empty interrupt */ + usart_interrupt_enable(UART_PERIF, USART_INT_TBE); + } +} +ssize_t _write(int fd, const void *ptr, size_t len) { + if (len > uartOutputBufferLength) { + len = uartOutputBufferLength; + } + outputLength = len; + currentOutputPos = 0; + memcpy(uartOutputBuffer, ptr, len); + /* enable USART1 Transmit Buffer Empty interrupt */ + usart_interrupt_enable(UART_PERIF, USART_INT_TBE); + delay_ms(1); + return len; +} +void USART1_IRQHandler(void) { + if (RESET != usart_interrupt_flag_get(UART_PERIF, USART_INT_FLAG_TBE)) { + /* write one byte to the transmit data register */ + usart_data_transmit(UART_PERIF, uartOutputBuffer[currentOutputPos++]); + if (currentOutputPos >= outputLength) { + currentOutputPos = 0xFF; // Mark done + usart_interrupt_disable(UART_PERIF, USART_INT_TBE); + } + } +} diff --git a/source/Core/BSP/Magic/Debug.h b/source/Core/BSP/Magic/Debug.h new file mode 100644 index 00000000..5c95a132 --- /dev/null +++ b/source/Core/BSP/Magic/Debug.h @@ -0,0 +1,22 @@ +/* + * Debug.h + * + * Created on: 26 Jan. 2021 + * Author: Ralim + */ + +#ifndef CORE_BSP_PINE64_DEBUG_H_ +#define CORE_BSP_PINE64_DEBUG_H_ + +#include "BSP.h" +#include "TipThermoModel.h" +#include +#include + +const unsigned int uartOutputBufferLength = 32; +extern char uartOutputBuffer[uartOutputBufferLength]; +extern "C" { +ssize_t _write(int fd, const void *ptr, size_t len); +void USART1_IRQHandler(void); +} +#endif /* CORE_BSP_PINE64_DEBUG_H_ */ diff --git a/source/Core/BSP/Magic/FreeRTOSConfig.h b/source/Core/BSP/Magic/FreeRTOSConfig.h new file mode 100644 index 00000000..91a5e4b6 --- /dev/null +++ b/source/Core/BSP/Magic/FreeRTOSConfig.h @@ -0,0 +1,91 @@ +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H +#include "nuclei_sdk_soc.h" +#include +#define configUSE_PREEMPTION 1 +#define configUSE_PORT_OPTIMISED_TASK_SELECTION 0 +#define configUSE_TICKLESS_IDLE 0 +#define configCPU_CLOCK_HZ ((uint32_t)SystemCoreClock) +#define configRTC_CLOCK_HZ ((uint32_t)32768) +#define configTICK_RATE_HZ ((TickType_t)1000) +#define configMAX_PRIORITIES (4) +#define configMINIMAL_STACK_SIZE ((unsigned short)128) +#define configMAX_TASK_NAME_LEN 24 +#define configUSE_16_BIT_TICKS 0 +#define configIDLE_SHOULD_YIELD 0 +#define configUSE_TASK_NOTIFICATIONS 1 +#define configUSE_MUTEXES 1 +#define configUSE_RECURSIVE_MUTEXES 0 +#define configUSE_COUNTING_SEMAPHORES 0 +#define configQUEUE_REGISTRY_SIZE 10 +#define configUSE_QUEUE_SETS 0 +#define configUSE_TIME_SLICING 1 +#define configUSE_NEWLIB_REENTRANT 0 +#define configENABLE_BACKWARD_COMPATIBILITY 0 + +#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_vTaskDelay 1 +/* Memory allocation related definitions. */ +#define configSUPPORT_STATIC_ALLOCATION 1 +#define configSUPPORT_DYNAMIC_ALLOCATION 0 +#define configTOTAL_HEAP_SIZE 1024 +#define configAPPLICATION_ALLOCATED_HEAP 0 + +/* Hook function related definitions. */ +#define configUSE_IDLE_HOOK 1 +#define configUSE_TICK_HOOK 0 +#define configCHECK_FOR_STACK_OVERFLOW 1 +#define configUSE_MALLOC_FAILED_HOOK 0 +#define configUSE_DAEMON_TASK_STARTUP_HOOK 0 + +/* Run time and task stats gathering related definitions. */ +#define configGENERATE_RUN_TIME_STATS 0 +#define configUSE_TRACE_FACILITY 0 +#define configUSE_STATS_FORMATTING_FUNCTIONS 0 + +/* Co-routine related definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES 1 + +/* Software timer related definitions. */ +#define configUSE_TIMERS 0 +#define configTIMER_TASK_PRIORITY 3 +#define configTIMER_QUEUE_LENGTH 5 +#define configTIMER_TASK_STACK_DEPTH configMINIMAL_STACK_SIZE + +/* Interrupt nesting behaviour configuration. */ +#define configPRIO_BITS (4UL) +#define configLIBRARY_LOWEST_INTERRUPT_PRIORITY 0x1 +#define configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY 0xe +#define configKERNEL_INTERRUPT_PRIORITY (configLIBRARY_LOWEST_INTERRUPT_PRIORITY << (8 - configPRIO_BITS)) +#define configMAX_SYSCALL_INTERRUPT_PRIORITY (configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY << (8 - configPRIO_BITS)) + +/* Define to trap errors during development. */ +#define configASSERT(x) \ + if ((x) == 0) { \ + taskDISABLE_INTERRUPTS(); \ + for (;;) \ + ; \ + } + +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_xResumeFromISR 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_xTaskGetIdleTaskHandle 1 +#define INCLUDE_eTaskGetState 0 +#define INCLUDE_xEventGroupSetBitFromISR 1 +#define INCLUDE_xTimerPendFunctionCall 0 +#define INCLUDE_xTaskAbortDelay 0 +#define INCLUDE_xTaskGetHandle 1 +#define INCLUDE_xTaskResumeFromISR 1 +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +#endif /* FREERTOS_CONFIG_H */ diff --git a/source/Core/BSP/Magic/I2C_Wrapper.cpp b/source/Core/BSP/Magic/I2C_Wrapper.cpp new file mode 100644 index 00000000..f54e0a42 --- /dev/null +++ b/source/Core/BSP/Magic/I2C_Wrapper.cpp @@ -0,0 +1,364 @@ +/* + * FRToSI2C.cpp + * + * Created on: 14Apr.,2018 + * Author: Ralim + */ +#include "BSP.h" +#include "IRQ.h" +#include "Setup.h" +#include + +SemaphoreHandle_t FRToSI2C::I2CSemaphore = nullptr; +StaticSemaphore_t FRToSI2C::xSemaphoreBuffer; +#define I2C_TIME_OUT (uint16_t)(12000) +void FRToSI2C::CpltCallback() { + // TODO +} + +bool FRToSI2C::I2C_RegisterWrite(uint8_t address, uint8_t reg, uint8_t data) { return Mem_Write(address, reg, &data, 1); } + +uint8_t FRToSI2C::I2C_RegisterRead(uint8_t add, uint8_t reg) { + uint8_t temp = 0; + Mem_Read(add, reg, &temp, 1); + return temp; +} + +enum class i2c_step { + // Write+read steps + Write_start, // Sending start on bus + Write_device_address, // start sent, send device address + Write_device_memory_address, // device address sent, write the memory location + Write_device_data_start, // Write all of the remaining data using DMA + Write_device_data_finish, // Write all of the remaining data using DMA + + Read_start, // second read + Read_device_address, // Send device address again for the read + Read_device_data_start, // read device data via DMA + Read_device_data_finish, // read device data via DMA + Send_stop, // send the stop at the end of the transaction + Wait_stop, // Wait for stop to send and we are done + Done, // Finished + Error_occured, // Error occured on the bus + +}; +struct i2c_state { + i2c_step currentStep; + bool isMemoryWrite; + bool wakePart; + uint8_t deviceAddress; + uint8_t memoryAddress; + uint8_t * buffer; + uint16_t numberOfBytes; + dma_parameter_struct dma_init_struct; +}; +i2c_state currentState; + +void perform_i2c_step() { + // Performs next step of the i2c state machine + if (i2c_flag_get(I2C0, I2C_FLAG_AERR)) { + i2c_flag_clear(I2C0, I2C_FLAG_AERR); + // Arb error - we lost the bus / nacked + currentState.currentStep = i2c_step::Error_occured; + } + switch (currentState.currentStep) { + case i2c_step::Error_occured: + i2c_stop_on_bus(I2C0); + break; + case i2c_step::Write_start: + + /* enable acknowledge */ + i2c_ack_config(I2C0, I2C_ACK_ENABLE); + /* i2c master sends start signal only when the bus is idle */ + if (!i2c_flag_get(I2C0, I2C_FLAG_I2CBSY)) { + /* send the start signal */ + i2c_start_on_bus(I2C0); + currentState.currentStep = i2c_step::Write_device_address; + } + break; + + case i2c_step::Write_device_address: + /* i2c master sends START signal successfully */ + if (i2c_flag_get(I2C0, I2C_FLAG_SBSEND)) { + i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND); // Clear sbsend by reading ctrl banks + i2c_master_addressing(I2C0, currentState.deviceAddress, I2C_TRANSMITTER); + currentState.currentStep = i2c_step::Write_device_memory_address; + } + break; + case i2c_step::Write_device_memory_address: + // Send the device memory location + + if (i2c_flag_get(I2C0, I2C_FLAG_ADDSEND)) { // addr sent + i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND); + + if (currentState.wakePart) { + // We are stopping here + currentState.currentStep = i2c_step::Send_stop; + return; + } + i2c_flag_clear(I2C0, I2C_FLAG_BTC); + // Write out the 8 byte address + i2c_data_transmit(I2C0, currentState.memoryAddress); + if (currentState.isMemoryWrite) { + currentState.currentStep = i2c_step::Write_device_data_start; + } else { + currentState.currentStep = i2c_step::Read_start; + } + } + + break; + case i2c_step::Write_device_data_start: + /* wait until the transmission data register is empty */ + if (i2c_flag_get(I2C0, I2C_FLAG_BTC)) { + dma_deinit(DMA0, DMA_CH5); + dma_init(DMA0, DMA_CH5, ¤tState.dma_init_struct); + i2c_dma_last_transfer_config(I2C0, I2C_DMALST_ON); + dma_circulation_disable(DMA0, DMA_CH5); + /* enable I2C0 DMA */ + i2c_dma_enable(I2C0, I2C_DMA_ON); + /* enable DMA0 channel5 */ + dma_channel_enable(DMA0, DMA_CH5); + currentState.currentStep = i2c_step::Write_device_data_finish; + } + break; + + case i2c_step::Write_device_data_finish: // Wait for complete then goto stop + /* wait until BTC bit is set */ + if (dma_flag_get(DMA0, DMA_CH5, DMA_FLAG_FTF)) { + /* wait until BTC bit is set */ + if (i2c_flag_get(I2C0, I2C_FLAG_BTC)) { + currentState.currentStep = i2c_step::Send_stop; + } + } + break; + case i2c_step::Read_start: + if (i2c_flag_get(I2C0, I2C_FLAG_BTC)) { + /* wait until BTC bit is set */ + i2c_start_on_bus(I2C0); + currentState.currentStep = i2c_step::Read_device_address; + } + break; + case i2c_step::Read_device_address: + if (i2c_flag_get(I2C0, I2C_FLAG_SBSEND)) { + i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND); + if (currentState.numberOfBytes == 1) { + /* disable acknowledge */ + i2c_master_addressing(I2C0, currentState.deviceAddress, I2C_RECEIVER); + while (!i2c_flag_get(I2C0, I2C_FLAG_ADDSEND)) {} + i2c_ack_config(I2C0, I2C_ACK_DISABLE); + i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND); + /* wait for the byte to be received */ + while (!i2c_flag_get(I2C0, I2C_FLAG_RBNE)) {} + /* read the byte received from the EEPROM */ + *currentState.buffer = i2c_data_receive(I2C0); + while (i2c_flag_get(I2C0, I2C_FLAG_RBNE)) { + i2c_data_receive(I2C0); + } + i2c_stop_on_bus(I2C0); + while ((I2C_CTL0(I2C0) & I2C_CTL0_STOP)) { + asm("nop"); + } + currentState.currentStep = i2c_step::Done; + } else if (currentState.numberOfBytes == 2) { + /* disable acknowledge */ + i2c_master_addressing(I2C0, currentState.deviceAddress, I2C_RECEIVER); + while (!i2c_flag_get(I2C0, I2C_FLAG_ADDSEND)) {} + i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND); + /* wait for the byte to be received */ + while (!i2c_flag_get(I2C0, I2C_FLAG_RBNE)) {} + i2c_ackpos_config(I2C0, I2C_ACKPOS_CURRENT); + i2c_ack_config(I2C0, I2C_ACK_DISABLE); + + /* read the byte received from the EEPROM */ + *currentState.buffer = i2c_data_receive(I2C0); + currentState.buffer++; + + /* wait for the byte to be received */ + while (!i2c_flag_get(I2C0, I2C_FLAG_RBNE)) {} + /* read the byte received from the EEPROM */ + *currentState.buffer = i2c_data_receive(I2C0); + while (i2c_flag_get(I2C0, I2C_FLAG_RBNE)) { + i2c_data_receive(I2C0); + } + i2c_stop_on_bus(I2C0); + while ((I2C_CTL0(I2C0) & I2C_CTL0_STOP)) { + asm("nop"); + } + currentState.currentStep = i2c_step::Done; + } else { + i2c_master_addressing(I2C0, currentState.deviceAddress, I2C_RECEIVER); + currentState.currentStep = i2c_step::Read_device_data_start; + } + } + break; + case i2c_step::Read_device_data_start: + if (i2c_flag_get(I2C0, I2C_FLAG_ADDSEND)) { // addr sent + i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND); + /* one byte master reception procedure (polling) */ + if (currentState.numberOfBytes == 0) { + currentState.currentStep = i2c_step::Send_stop; + } else { /* more than one byte master reception procedure (DMA) */ + + while (currentState.numberOfBytes) { + + if (3 == currentState.numberOfBytes) { + /* wait until BTC bit is set */ + while (!i2c_flag_get(I2C0, I2C_FLAG_BTC)) {} + i2c_ackpos_config(I2C0, I2C_ACKPOS_CURRENT); + /* disable acknowledge */ + i2c_ack_config(I2C0, I2C_ACK_DISABLE); + } else if (2 == currentState.numberOfBytes) { + /* wait until BTC bit is set */ + while (!i2c_flag_get(I2C0, I2C_FLAG_BTC)) {} + /* disable acknowledge */ + i2c_ack_config(I2C0, I2C_ACK_DISABLE); + /* send a stop condition to I2C bus */ + i2c_stop_on_bus(I2C0); + } + /* wait until RBNE bit is set */ + while (!i2c_flag_get(I2C0, I2C_FLAG_RBNE)) {} + /* read a byte from the EEPROM */ + *currentState.buffer = i2c_data_receive(I2C0); + + /* point to the next location where the byte read will be saved */ + currentState.buffer++; + + /* decrement the read bytes counter */ + currentState.numberOfBytes--; + } + currentState.currentStep = i2c_step::Wait_stop; + // currentState.currentStep = i2c_step::Read_device_data_finish; + } + } + break; + case i2c_step::Read_device_data_finish: // Wait for complete then goto stop + /* wait until BTC bit is set */ + + break; + case i2c_step::Send_stop: + /* send a stop condition to I2C bus*/ + i2c_stop_on_bus(I2C0); + currentState.currentStep = i2c_step::Wait_stop; + break; + case i2c_step::Wait_stop: + /* i2c master sends STOP signal successfully */ + if ((I2C_CTL0(I2C0) & I2C_CTL0_STOP) != I2C_CTL0_STOP) { + currentState.currentStep = i2c_step::Done; + } + break; + default: + // If we get here something is amiss + return; + } +} + +bool perform_i2c_transaction(uint16_t DevAddress, uint16_t memory_address, uint8_t *p_buffer, uint16_t number_of_byte, bool isWrite, bool isWakeOnly) { + + currentState.isMemoryWrite = isWrite; + currentState.wakePart = isWakeOnly; + currentState.deviceAddress = DevAddress; + currentState.memoryAddress = memory_address; + currentState.numberOfBytes = number_of_byte; + currentState.buffer = p_buffer; + // Setup DMA + currentState.dma_init_struct.memory_width = DMA_MEMORY_WIDTH_8BIT; + currentState.dma_init_struct.memory_addr = (uint32_t)p_buffer; + currentState.dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE; + currentState.dma_init_struct.number = number_of_byte; + currentState.dma_init_struct.periph_addr = (uint32_t)&I2C_DATA(I2C0); + currentState.dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE; + currentState.dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_8BIT; + currentState.dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH; + + if (currentState.isMemoryWrite) { + currentState.dma_init_struct.direction = DMA_MEMORY_TO_PERIPHERAL; + } else { + currentState.dma_init_struct.direction = DMA_PERIPHERAL_TO_MEMORY; + } + // Clear flags + I2C_STAT0(I2C0) = 0; + I2C_STAT1(I2C0) = 0; + i2c_flag_clear(I2C0, I2C_FLAG_ADDSEND); + i2c_ackpos_config(I2C0, I2C_ACKPOS_CURRENT); + i2c_data_receive(I2C0); + i2c_data_receive(I2C0); + currentState.currentStep = i2c_step::Write_start; // Always start in write mode + TickType_t timeout = xTaskGetTickCount() + TICKS_100MS; + while ((currentState.currentStep != i2c_step::Done) && (currentState.currentStep != i2c_step::Error_occured)) { + if (xTaskGetTickCount() > timeout) { + i2c_stop_on_bus(I2C0); + return false; + } + perform_i2c_step(); + } + return currentState.currentStep == i2c_step::Done; +} + +bool FRToSI2C::Mem_Read(uint16_t DevAddress, uint16_t read_address, uint8_t *p_buffer, uint16_t number_of_byte) { + if (!lock()) + return false; + bool res = perform_i2c_transaction(DevAddress, read_address, p_buffer, number_of_byte, false, false); + if (!res) { + I2C_Unstick(); + } + unlock(); + return res; +} + +bool FRToSI2C::Mem_Write(uint16_t DevAddress, uint16_t MemAddress, uint8_t *p_buffer, uint16_t number_of_byte) { + if (!lock()) + return false; + bool res = perform_i2c_transaction(DevAddress, MemAddress, p_buffer, number_of_byte, true, false); + if (!res) { + I2C_Unstick(); + } + unlock(); + return res; +} + +bool FRToSI2C::Transmit(uint16_t DevAddress, uint8_t *pData, uint16_t Size) { return Mem_Write(DevAddress, pData[0], pData + 1, Size - 1); } + +bool FRToSI2C::probe(uint16_t DevAddress) { + uint8_t temp[1]; + return Mem_Read(DevAddress, 0x00, temp, sizeof(temp)); +} + +void FRToSI2C::I2C_Unstick() { unstick_I2C(); } + +bool FRToSI2C::lock() { + if (I2CSemaphore == nullptr) { + return false; + } + return xSemaphoreTake(I2CSemaphore, TICKS_SECOND) == pdTRUE; +} + +void FRToSI2C::unlock() { xSemaphoreGive(I2CSemaphore); } + +bool FRToSI2C::writeRegistersBulk(const uint8_t address, const I2C_REG *registers, const uint8_t registersLength) { + for (int index = 0; index < registersLength; index++) { + if (!I2C_RegisterWrite(address, registers[index].reg, registers[index].val)) { + return false; + } + if (registers[index].pause_ms) { + delay_ms(registers[index].pause_ms); + } + } + return true; +} + +bool FRToSI2C::wakePart(uint16_t DevAddress) { + // wakepart is a special case where only the device address is sent + if (!lock()) + return false; + bool res = perform_i2c_transaction(DevAddress, 0, NULL, 0, false, true); + if (!res) { + I2C_Unstick(); + } + unlock(); + return res; +} + +void I2C_EV_IRQ() {} +void I2C_ER_IRQ() { + // Error callbacks +} diff --git a/source/Core/BSP/Magic/IRQ.cpp b/source/Core/BSP/Magic/IRQ.cpp new file mode 100644 index 00000000..6aa77aef --- /dev/null +++ b/source/Core/BSP/Magic/IRQ.cpp @@ -0,0 +1,119 @@ +/* + * IRQ.c + * + * Created on: 30 May 2020 + * Author: Ralim + */ + +#include "IRQ.h" +#include "Pins.h" +#include "configuration.h" +volatile uint8_t i2c_read_process = 0; +volatile uint8_t i2c_write_process = 0; +volatile uint8_t i2c_slave_address = 0; +volatile uint8_t i2c_error_code = 0; +volatile uint8_t *i2c_write; +volatile uint8_t *i2c_read; +volatile uint16_t i2c_nbytes; +volatile uint16_t i2c_write_dress; +volatile uint16_t i2c_read_dress; +volatile uint8_t i2c_process_flag = 0; +static bool fastPWM; +static void switchToSlowPWM(void); +static void switchToFastPWM(void); +void ADC0_1_IRQHandler(void) { + + adc_interrupt_flag_clear(ADC0, ADC_INT_FLAG_EOIC); + // unblock the PID controller thread + if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) { + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + if (pidTaskNotification) { + vTaskNotifyGiveFromISR(pidTaskNotification, &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + } + } +} + +volatile uint16_t PWMSafetyTimer = 0; +volatile uint8_t pendingPWM = 0; +void TIMER1_IRQHandler(void) { + static bool lastPeriodWasFast = false; + + if (timer_interrupt_flag_get(TIMER1, TIMER_INT_UP) == SET) { + timer_interrupt_flag_clear(TIMER1, TIMER_INT_UP); + // rollover turn on output if required + if (PWMSafetyTimer) { + PWMSafetyTimer--; + if (lastPeriodWasFast != fastPWM) { + if (fastPWM) { + switchToFastPWM(); + } else { + switchToSlowPWM(); + } + } + if (pendingPWM) { + timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_1, pendingPWM); + timer_channel_output_pulse_value_config(TIMER2, TIMER_CH_0, 50); + } else { + timer_channel_output_pulse_value_config(TIMER2, TIMER_CH_0, 0); + } + } + } + if (timer_interrupt_flag_get(TIMER1, TIMER_INT_CH1) == SET) { + timer_interrupt_flag_clear(TIMER1, TIMER_INT_CH1); + timer_channel_output_pulse_value_config(TIMER2, TIMER_CH_0, 0); + } +} + +void switchToFastPWM(void) { + fastPWM = true; + totalPWM = powerPWM + tempMeasureTicks + holdoffTicks; + TIMER_CAR(TIMER1) = (uint32_t)totalPWM; + + // ~10Hz + TIMER_CH0CV(TIMER1) = powerPWM + holdoffTicks; + // 1 kHz tick rate + TIMER_PSC(TIMER1) = 18000; +} + +void switchToSlowPWM(void) { + // 5Hz + fastPWM = false; + totalPWM = powerPWM + tempMeasureTicks / 2 + holdoffTicks / 2; + TIMER_CAR(TIMER1) = (uint32_t)totalPWM; + TIMER_CH0CV(TIMER1) = powerPWM + holdoffTicks / 2; + TIMER_PSC(TIMER1) = 36000; +} +void setTipPWM(const uint8_t pulse, const bool shouldUseFastModePWM) { + PWMSafetyTimer = 10; // This is decremented in the handler for PWM so that the tip pwm is + // disabled if the PID task is not scheduled often enough. + pendingPWM = pulse; + fastPWM = shouldUseFastModePWM; +} +extern osThreadId POWTaskHandle; + +void EXTI5_9_IRQHandler(void) { +#if POW_PD + if (RESET != exti_interrupt_flag_get(EXTI_5)) { + exti_interrupt_flag_clear(EXTI_5); + + if (POWTaskHandle != nullptr) { + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xTaskNotifyFromISR(POWTaskHandle, 1, eSetBits, &xHigherPriorityTaskWoken); + /* Force a context switch if xHigherPriorityTaskWoken is now set to pdTRUE. + The macro used to do this is dependent on the port and may be called + portEND_SWITCHING_ISR. */ + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + } + } +#endif +} + +bool getFUS302IRQLow() { + // Return true if the IRQ line is still held low + return (RESET == gpio_input_bit_get(FUSB302_IRQ_GPIO_Port, FUSB302_IRQ_Pin)); +} +// These are unused for now +void I2C0_EV_IRQHandler(void) {} + +void I2C0_ER_IRQHandler(void) {} diff --git a/source/Core/BSP/Magic/IRQ.h b/source/Core/BSP/Magic/IRQ.h new file mode 100644 index 00000000..b3d27122 --- /dev/null +++ b/source/Core/BSP/Magic/IRQ.h @@ -0,0 +1,56 @@ +/* + * Irqs.h + * + * Created on: 30 May 2020 + * Author: Ralim + */ + +#ifndef BSP_PINE64_IRQ_H_ +#define BSP_PINE64_IRQ_H_ + +#include "BSP.h" +#include "I2C_Wrapper.hpp" +#include "Setup.h" +#include "gd32vf103.h" +#include "main.hpp" + +#ifdef __cplusplus +extern "C" { +#endif +void ADC0_1_IRQHandler(void); +void TIMER1_IRQHandler(void); +void EXTI5_9_IRQHandler(void); +/* handle I2C0 event interrupt request */ +void I2C0_EV_IRQHandler(void); +/* handle I2C0 error interrupt request */ +void I2C0_ER_IRQHandler(void); +typedef enum { + I2C_SEND_ADDRESS_FIRST = 0, // Sending slave address + I2C_CLEAR_ADDRESS_FLAG_FIRST, // Clear address send + I2C_TRANSMIT_WRITE_READ_ADD, // Transmit the memory address to read/write from + I2C_SEND_ADDRESS_SECOND, // Send address again for read + I2C_CLEAR_ADDRESS_FLAG_SECOND, // Clear address again + I2C_TRANSMIT_DATA, // Transmit recieve data + I2C_STOP, // Send stop + I2C_ABORTED, // + I2C_DONE, // I2C transfer is complete + I2C_START, + I2C_END, + I2C_OK, + I2C_SEND_ADDRESS, + I2C_CLEAR_ADDRESS_FLAG, +} i2c_process_enum; +extern volatile uint8_t i2c_slave_address; +extern volatile uint8_t i2c_read_process; +extern volatile uint8_t i2c_write_process; +extern volatile uint8_t i2c_error_code; +extern volatile uint8_t *i2c_write; +extern volatile uint8_t *i2c_read; +extern volatile uint16_t i2c_nbytes; +extern volatile uint16_t i2c_write_dress; +extern volatile uint16_t i2c_read_dress; +extern volatile uint8_t i2c_process_flag; +#ifdef __cplusplus +} +#endif +#endif /* BSP_PINE64_IRQ_H_ */ diff --git a/source/Core/BSP/Magic/NOTES.md b/source/Core/BSP/Magic/NOTES.md new file mode 100644 index 00000000..2482821e --- /dev/null +++ b/source/Core/BSP/Magic/NOTES.md @@ -0,0 +1,38 @@ +# Notes on RISC-V + +## Pinmap + +| Pin Number | Name | Function | Notes | +| ---------- | ---- | ---------------- | ----------- | +| 17 | PB2 | BOOT2 | Pulldown | +| 32 | | IMU INT 1 | N/A | +| 30 | | IMU INT 2 | N/A | +| | PA4 | Handle Temp | ADC Input ? | +| | PA1 | Tip Temp | ADC Input ? | +| | PB1 | B Button | Active High | +| | PB0 | A Button | Active High | +| | PA11 | USB D- | - | +| | PA12 | USB D+ | - | +| | PA6 | Tip PWM Out | - | +| | PA0 | Input DC V Sense | ADC Input ? | +| | PA9 | OLED Reset | | +| | PB7 | SDA | I2C0_SDA | +| | PB6 | SCL | I2C0_SCL | + +## ADC Configuration + +For now running in matching mode for TS100 + +- X channels DMA in background +- Sample tip using "Intereted" channels using TIMER 0,1,3 TRGO or timer0,1,2 channels +- Using just 12 bit mode for now and move to hardware oversampling later +- use DMA for normal samples and 4x16 bit regs for tip temp +- It has dual ADC's so run them in pair mode + +## Timers + +### Timer 2 + +Timer 2 CH0 is tip drive PWM out. +This is fixed at 50% duty cycle and used via the cap to turn on the heater tip. +This should toggle relatively quickly. diff --git a/source/Core/BSP/Magic/Pins.h b/source/Core/BSP/Magic/Pins.h new file mode 100644 index 00000000..09d3e9e3 --- /dev/null +++ b/source/Core/BSP/Magic/Pins.h @@ -0,0 +1,60 @@ +/* + * Pins.h + * + * Created on: 29 May 2020 + * Author: Ralim + */ + +#ifndef BSP_PINE64_PINS_H_ +#define BSP_PINE64_PINS_H_ +#include "gd32vf103_gpio.h" + +#define KEY_B_Pin BIT(1) +#define KEY_B_GPIO_Port GPIOB +#define TMP36_INPUT_Pin BIT(4) +#define TMP36_INPUT_GPIO_Port GPIOA +#define TMP36_ADC0_CHANNEL ADC_CHANNEL_4 +#define TMP36_ADC1_CHANNEL ADC_CHANNEL_4 +#define TIP_TEMP_Pin BIT(1) +#define TIP_TEMP_GPIO_Port GPIOA +#define TIP_TEMP_ADC0_CHANNEL ADC_CHANNEL_1 +#define TIP_TEMP_ADC1_CHANNEL ADC_CHANNEL_1 + +#define VIN_Pin BIT(0) +#define VIN_GPIO_Port GPIOA +#define VIN_ADC0_CHANNEL ADC_CHANNEL_0 +#define VIN_ADC1_CHANNEL ADC_CHANNEL_0 +#define OLED_RESET_Pin BIT(9) +#define OLED_RESET_GPIO_Port GPIOA +#define KEY_A_Pin BIT(0) +#define KEY_A_GPIO_Port GPIOB +#define PWM_Out_Pin BIT(6) +#define PWM_Out_GPIO_Port GPIOA +#define SCL_Pin BIT(6) +#define SCL_GPIO_Port GPIOB +#define SDA_Pin BIT(7) +#define SDA_GPIO_Port GPIOB + +#define USB_DM_Pin BIT(11) +#define USB_DM_LOW_GPIO_Port GPIOA + +#define QC_DP_LOW_Pin BIT(7) +#define QC_DP_LOW_GPIO_Port GPIOA + +// LOW = low resistance, HIGH = high resistance +#define QC_DM_LOW_Pin BIT(8) +#define QC_DM_LOW_GPIO_Port GPIOA +#define QC_DM_HIGH_Pin BIT(10) +#define QC_DM_HIGH_GPIO_Port GPIOA + +#define FUSB302_IRQ_Pin BIT(5) +#define FUSB302_IRQ_GPIO_Port GPIOB + +// uart +#define UART_TX_Pin BIT(2) +#define UART_TX_GPIO_Port GPIOA +#define UART_RX_Pin BIT(3) +#define UART_RX_GPIO_Port GPIOA +#define UART_PERIF USART1 + +#endif /* BSP_PINE64_PINS_H_ */ diff --git a/source/Core/BSP/Magic/Power.cpp b/source/Core/BSP/Magic/Power.cpp new file mode 100644 index 00000000..f10dfacd --- /dev/null +++ b/source/Core/BSP/Magic/Power.cpp @@ -0,0 +1,40 @@ +#include "BSP.h" +#include "BSP_Power.h" +#include "Pins.h" +#include "QC3.h" +#include "Settings.h" +#include "USBPD.h" +#include "configuration.h" + +void power_check() { +#if POW_PD + // Cant start QC until either PD works or fails + if (!USBPowerDelivery::negotiationComplete()) { + return; + } + if (USBPowerDelivery::negotiationHasWorked()) { + return; // We are using PD + } +#endif +#ifdef POW_QC + QC_resync(); +#endif +} + +bool getIsPoweredByDCIN() { +#if POW_PD + if (!USBPowerDelivery::negotiationComplete()) { + return false; // We are assuming not dc while negotiating + } + if (USBPowerDelivery::negotiationHasWorked()) { + return false; // We are using PD + } +#endif + +#ifdef POW_QC + if (hasQCNegotiated()) { + return false; // We are using QC + } +#endif + return true; +} diff --git a/source/Core/BSP/Magic/QC_GPIO.cpp b/source/Core/BSP/Magic/QC_GPIO.cpp new file mode 100644 index 00000000..11ad5344 --- /dev/null +++ b/source/Core/BSP/Magic/QC_GPIO.cpp @@ -0,0 +1,51 @@ +/* + * QC.c + * + * Created on: 29 May 2020 + * Author: Ralim + */ +#include "BSP.h" +#include "Pins.h" +#include "QC3.h" +#include "Settings.h" +#include "gd32vf103_libopt.h" + +#ifdef POW_QC +void QC_DPlusZero_Six() { + // pull down D+ + gpio_bit_reset(QC_DP_LOW_GPIO_Port, QC_DP_LOW_Pin); +} +void QC_DNegZero_Six() { + gpio_bit_set(QC_DM_HIGH_GPIO_Port, QC_DM_HIGH_Pin); + gpio_bit_reset(QC_DM_LOW_GPIO_Port, QC_DM_LOW_Pin); +} +void QC_DPlusThree_Three() { + // pull up D+ + gpio_bit_set(QC_DP_LOW_GPIO_Port, QC_DP_LOW_Pin); +} +void QC_DNegThree_Three() { + gpio_bit_set(QC_DM_LOW_GPIO_Port, QC_DM_LOW_Pin); + gpio_bit_set(QC_DM_HIGH_GPIO_Port, QC_DM_HIGH_Pin); +} +void QC_DM_PullDown() { gpio_init(USB_DM_LOW_GPIO_Port, GPIO_MODE_IPD, GPIO_OSPEED_2MHZ, USB_DM_Pin); } +void QC_DM_No_PullDown() { gpio_init(USB_DM_LOW_GPIO_Port, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_2MHZ, USB_DM_Pin); } +void QC_Init_GPIO() { + // Setup any GPIO into the right states for QC + // D+ pulldown as output + gpio_init(QC_DP_LOW_GPIO_Port, GPIO_MODE_OUT_PP, GPIO_OSPEED_2MHZ, QC_DP_LOW_Pin); + // Make two D- pins floating + QC_DM_PullDown(); +} +void QC_Post_Probe_En() { + // Make two D- pins outputs + gpio_init(QC_DM_LOW_GPIO_Port, GPIO_MODE_OUT_PP, GPIO_OSPEED_2MHZ, QC_DM_LOW_Pin); + gpio_init(QC_DM_HIGH_GPIO_Port, GPIO_MODE_OUT_PP, GPIO_OSPEED_2MHZ, QC_DM_HIGH_Pin); +} + +uint8_t QC_DM_PulledDown() { return gpio_input_bit_get(USB_DM_LOW_GPIO_Port, USB_DM_Pin) == RESET ? 1 : 0; } +#endif +void QC_resync() { +#ifdef POW_QC + seekQC(getSettingValue(SettingsOptions::QCIdealVoltage), getSettingValue(SettingsOptions::VoltageDiv)); // Run the QC seek again if we have drifted too much +#endif +} diff --git a/source/Core/BSP/Magic/README.md b/source/Core/BSP/Magic/README.md new file mode 100644 index 00000000..944507c4 --- /dev/null +++ b/source/Core/BSP/Magic/README.md @@ -0,0 +1,3 @@ +# BSP section for Pinecil + +This folder contains the hardware abstractions required for the Pinecil. A RISC-V based soldering iron. diff --git a/source/Core/BSP/Magic/Setup.cpp b/source/Core/BSP/Magic/Setup.cpp new file mode 100644 index 00000000..a0b33c63 --- /dev/null +++ b/source/Core/BSP/Magic/Setup.cpp @@ -0,0 +1,342 @@ +/* + * Setup.c + * + * Created on: 29Aug.,2017 + * Author: Ben V. Brown + */ +#include "Setup.h" +#include "BSP.h" +#include "Debug.h" +#include "Pins.h" +#include "gd32vf103.h" +#include "history.hpp" +#include +#define ADC_NORM_SAMPLES 16 +#define ADC_FILTER_LEN 4 +uint16_t ADCReadings[ADC_NORM_SAMPLES]; // room for 32 lots of the pair of readings + +// Functions +void setup_gpio(); +void setup_dma(); +void setup_i2c(); +void setup_adc(); +void setup_timers(); +void setup_iwdg(); +void setup_uart(); + +void hardware_init() { + // I2C + setup_i2c(); + // GPIO + setup_gpio(); + // DMA + setup_dma(); + // ADC's + setup_adc(); + // Timers + setup_timers(); + // Watchdog + setup_iwdg(); + // ELIC + eclic_priority_group_set(ECLIC_PRIGROUP_LEVEL0_PRIO4); + // uart for debugging + setup_uart(); + /* enable TIMER1 - PWM control timing*/ + timer_enable(TIMER1); + timer_enable(TIMER2); +} + +uint16_t getADCHandleTemp(uint8_t sample) { + static history filter = {{0}, 0, 0}; + if (sample) { + uint32_t sum = 0; + for (uint8_t i = 0; i < ADC_NORM_SAMPLES; i++) { + sum += ADCReadings[i]; + } + filter.update(sum); + } + return filter.average() >> 1; +} + +uint16_t getADCVin(uint8_t sample) { + static history filter = {{0}, 0, 0}; + if (sample) { + uint16_t latestADC = 0; + + latestADC += adc_inserted_data_read(ADC1, 0); + latestADC += adc_inserted_data_read(ADC1, 1); + latestADC += adc_inserted_data_read(ADC1, 2); + latestADC += adc_inserted_data_read(ADC1, 3); + latestADC <<= 1; + filter.update(latestADC); + } + return filter.average(); +} +// Returns either average or instant value. When sample is set the samples from the injected ADC are copied to the filter and then the raw reading is returned +uint16_t getTipRawTemp(uint8_t sample) { + static history filter = {{0}, 0, 0}; + if (sample) { + uint16_t latestADC = 0; + + latestADC += adc_inserted_data_read(ADC0, 0); + latestADC += adc_inserted_data_read(ADC0, 1); + latestADC += adc_inserted_data_read(ADC0, 2); + latestADC += adc_inserted_data_read(ADC0, 3); + latestADC <<= 1; + filter.update(latestADC); + return latestADC; + } + return filter.average(); +} + +void setup_uart() { + // Setup the uart pins as a uart with dma + + /* enable USART clock */ + rcu_periph_clock_enable(RCU_USART1); + + /* connect port to USARTx_Tx */ + gpio_init(UART_TX_GPIO_Port, GPIO_MODE_AF_PP, GPIO_OSPEED_10MHZ, UART_TX_Pin); + + /* connect port to USARTx_Rx */ + gpio_init(UART_RX_GPIO_Port, GPIO_MODE_IPU, GPIO_OSPEED_10MHZ, UART_RX_Pin); + + /* USART configure */ + usart_deinit(UART_PERIF); + usart_baudrate_set(UART_PERIF, 1000000); + usart_word_length_set(UART_PERIF, USART_WL_8BIT); + usart_stop_bit_set(UART_PERIF, USART_STB_1BIT); + usart_parity_config(UART_PERIF, USART_PM_NONE); + usart_hardware_flow_rts_config(UART_PERIF, USART_RTS_DISABLE); + usart_hardware_flow_cts_config(UART_PERIF, USART_CTS_DISABLE); + usart_receive_config(UART_PERIF, USART_RECEIVE_DISABLE); // Dont use rx for now + usart_transmit_config(UART_PERIF, USART_TRANSMIT_ENABLE); + eclic_irq_enable(USART1_IRQn, 15, 15); + usart_enable(UART_PERIF); +} + +void setup_gpio() { + /* enable GPIOB clock */ + rcu_periph_clock_enable(RCU_GPIOA); + /* enable GPIOB clock */ + rcu_periph_clock_enable(RCU_GPIOB); + // Alternate function clock enable + rcu_periph_clock_enable(RCU_AF); + // Buttons as input + gpio_init(KEY_A_GPIO_Port, GPIO_MODE_IPD, GPIO_OSPEED_2MHZ, KEY_A_Pin); + gpio_init(KEY_B_GPIO_Port, GPIO_MODE_IPD, GPIO_OSPEED_2MHZ, KEY_B_Pin); + // OLED reset as output + gpio_init(OLED_RESET_GPIO_Port, GPIO_MODE_OUT_PP, GPIO_OSPEED_2MHZ, OLED_RESET_Pin); + // I2C as AF Open Drain + gpio_init(SDA_GPIO_Port, GPIO_MODE_AF_OD, GPIO_OSPEED_50MHZ, SDA_Pin); + gpio_init(SCL_GPIO_Port, GPIO_MODE_AF_OD, GPIO_OSPEED_50MHZ, SCL_Pin); + // PWM output as AF Push Pull + gpio_init(PWM_Out_GPIO_Port, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, PWM_Out_Pin); + // Analog Inputs ... as analog inputs + gpio_init(TMP36_INPUT_GPIO_Port, GPIO_MODE_AIN, GPIO_OSPEED_2MHZ, TMP36_INPUT_Pin); + gpio_init(TIP_TEMP_GPIO_Port, GPIO_MODE_AIN, GPIO_OSPEED_2MHZ, TIP_TEMP_Pin); + gpio_init(VIN_GPIO_Port, GPIO_MODE_AIN, GPIO_OSPEED_2MHZ, VIN_Pin); + + // Remap PB4 away from JTAG NJRST + gpio_pin_remap_config(GPIO_SWJ_NONJTRST_REMAP, ENABLE); + // FUSB interrupt + gpio_init(FUSB302_IRQ_GPIO_Port, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, FUSB302_IRQ_Pin); +} +void setup_dma() { + // Setup DMA for ADC0 + { + /* enable DMA0 clock */ + rcu_periph_clock_enable(RCU_DMA0); + // rcu_periph_clock_enable(RCU_DMA1); + /* ADC_DMA_channel configuration */ + dma_parameter_struct dma_data_parameter; + + /* ADC DMA_channel configuration */ + dma_deinit(DMA0, DMA_CH0); + + /* initialize DMA data mode */ + dma_data_parameter.periph_addr = (uint32_t)(&ADC_RDATA(ADC0)); + dma_data_parameter.periph_inc = DMA_PERIPH_INCREASE_DISABLE; + dma_data_parameter.memory_addr = (uint32_t)(ADCReadings); + dma_data_parameter.memory_inc = DMA_MEMORY_INCREASE_ENABLE; + dma_data_parameter.periph_width = DMA_PERIPHERAL_WIDTH_16BIT; + dma_data_parameter.memory_width = DMA_MEMORY_WIDTH_16BIT; + dma_data_parameter.direction = DMA_PERIPHERAL_TO_MEMORY; + dma_data_parameter.number = ADC_NORM_SAMPLES; + dma_data_parameter.priority = DMA_PRIORITY_HIGH; + dma_init(DMA0, DMA_CH0, &dma_data_parameter); + + dma_circulation_enable(DMA0, DMA_CH0); + + /* enable DMA channel */ + dma_channel_enable(DMA0, DMA_CH0); + } +} +void setup_i2c() { + /* enable I2C0 clock */ + rcu_periph_clock_enable(RCU_I2C0); + /* enable DMA0 clock */ + rcu_periph_clock_enable(RCU_DMA0); + // Setup I20 at 400kHz + i2c_clock_config(I2C0, 400 * 1000, I2C_DTCY_2); + i2c_mode_addr_config(I2C0, I2C_I2CMODE_ENABLE, I2C_ADDFORMAT_7BITS, 0x7F); + i2c_enable(I2C0); + /* enable acknowledge */ + i2c_ack_config(I2C0, I2C_ACK_ENABLE); +} +void setup_adc() { + + // Setup ADC in normal + injected mode + // Want it to sample handle temp and input voltage normally via dma + // Then injected trigger to sample tip temp + memset(ADCReadings, 0, sizeof(ADCReadings)); + rcu_periph_clock_enable(RCU_ADC0); + rcu_periph_clock_enable(RCU_ADC1); + adc_deinit(ADC0); + adc_deinit(ADC1); + /* config ADC clock */ + rcu_adc_clock_config(RCU_CKADC_CKAPB2_DIV16); + // Run in normal parallel + inserted parallel + adc_mode_config(ADC_DAUL_INSERTED_PARALLEL); + adc_special_function_config(ADC0, ADC_CONTINUOUS_MODE, ENABLE); + adc_special_function_config(ADC0, ADC_SCAN_MODE, ENABLE); + adc_special_function_config(ADC1, ADC_CONTINUOUS_MODE, ENABLE); + adc_special_function_config(ADC1, ADC_SCAN_MODE, ENABLE); + // Align right + adc_data_alignment_config(ADC0, ADC_DATAALIGN_RIGHT); + adc_data_alignment_config(ADC1, ADC_DATAALIGN_RIGHT); + // Setup reading the handle temp + adc_channel_length_config(ADC0, ADC_REGULAR_CHANNEL, 1); + adc_channel_length_config(ADC1, ADC_REGULAR_CHANNEL, 0); + // Setup the two channels + adc_regular_channel_config(ADC0, 0, TMP36_ADC0_CHANNEL, + ADC_SAMPLETIME_71POINT5); // temp sensor + // Setup that we want all 4 inserted readings to be the tip temp + adc_channel_length_config(ADC0, ADC_INSERTED_CHANNEL, 4); + adc_channel_length_config(ADC1, ADC_INSERTED_CHANNEL, 4); + for (int rank = 0; rank < 4; rank++) { + adc_inserted_channel_config(ADC0, rank, TIP_TEMP_ADC0_CHANNEL, ADC_SAMPLETIME_28POINT5); + adc_inserted_channel_config(ADC1, rank, VIN_ADC1_CHANNEL, ADC_SAMPLETIME_28POINT5); + } + // Setup timer 1 channel 0 to trigger injected measurements + adc_external_trigger_source_config(ADC0, ADC_INSERTED_CHANNEL, ADC0_1_EXTTRIG_INSERTED_T1_TRGO); + adc_external_trigger_source_config(ADC1, ADC_INSERTED_CHANNEL, ADC0_1_EXTTRIG_INSERTED_T1_TRGO); + + adc_external_trigger_source_config(ADC0, ADC_REGULAR_CHANNEL, ADC0_1_EXTTRIG_REGULAR_NONE); + adc_external_trigger_source_config(ADC1, ADC_REGULAR_CHANNEL, ADC0_1_EXTTRIG_REGULAR_NONE); + // Enable triggers for the ADC + adc_external_trigger_config(ADC0, ADC_INSERTED_CHANNEL, ENABLE); + adc_external_trigger_config(ADC1, ADC_INSERTED_CHANNEL, ENABLE); + adc_external_trigger_config(ADC0, ADC_REGULAR_CHANNEL, ENABLE); + adc_external_trigger_config(ADC1, ADC_REGULAR_CHANNEL, ENABLE); + + adc_watchdog_disable(ADC0); + adc_watchdog_disable(ADC1); + adc_resolution_config(ADC0, ADC_RESOLUTION_12B); + adc_resolution_config(ADC1, ADC_RESOLUTION_12B); + /* clear the ADC flag */ + adc_oversample_mode_disable(ADC0); + adc_oversample_mode_disable(ADC1); + adc_enable(ADC0); + adc_calibration_enable(ADC0); + adc_enable(ADC1); + adc_calibration_enable(ADC1); + adc_dma_mode_enable(ADC0); + // Enable interrupt on end of injected readings + adc_interrupt_flag_clear(ADC0, ADC_INT_FLAG_EOC); + adc_interrupt_flag_clear(ADC0, ADC_INT_FLAG_EOIC); + adc_interrupt_enable(ADC0, ADC_INT_EOIC); + eclic_irq_enable(ADC0_1_IRQn, 2, 0); + adc_software_trigger_enable(ADC0, ADC_REGULAR_CHANNEL); + adc_software_trigger_enable(ADC1, ADC_REGULAR_CHANNEL); + adc_tempsensor_vrefint_disable(); +} +void setup_timers() { + // Setup timer 1 to run the actual PWM level + /* enable timer1 clock */ + rcu_periph_clock_enable(RCU_TIMER1); + rcu_periph_clock_enable(RCU_TIMER2); + timer_oc_parameter_struct timer_ocintpara; + timer_parameter_struct timer_initpara; + { + // deinit to reset the timer + timer_deinit(TIMER1); + /* initialize TIMER init parameter struct */ + timer_struct_para_init(&timer_initpara); + /* TIMER1 configuration */ + timer_initpara.prescaler = 30000; + timer_initpara.alignedmode = TIMER_COUNTER_EDGE; + timer_initpara.counterdirection = TIMER_COUNTER_UP; + timer_initpara.period = powerPWM + tempMeasureTicks + holdoffTicks; + timer_initpara.clockdivision = TIMER_CKDIV_DIV4; + timer_initpara.repetitioncounter = 0; + timer_init(TIMER1, &timer_initpara); + + /* CH0 configured to implement the PWM irq's for the output control*/ + timer_channel_output_struct_para_init(&timer_ocintpara); + timer_ocintpara.ocpolarity = TIMER_OC_POLARITY_LOW; + timer_ocintpara.outputstate = TIMER_CCX_ENABLE; + timer_channel_output_config(TIMER1, TIMER_CH_0, &timer_ocintpara); + + timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_0, powerPWM + holdoffTicks); + timer_channel_output_mode_config(TIMER1, TIMER_CH_0, TIMER_OC_MODE_PWM1); + timer_channel_output_shadow_config(TIMER1, TIMER_CH_0, TIMER_OC_SHADOW_DISABLE); + /* CH1 used for irq */ + timer_channel_output_struct_para_init(&timer_ocintpara); + timer_ocintpara.ocpolarity = TIMER_OC_POLARITY_HIGH; + timer_ocintpara.outputstate = TIMER_CCX_ENABLE; + timer_channel_output_config(TIMER1, TIMER_CH_1, &timer_ocintpara); + timer_master_output_trigger_source_select(TIMER1, TIMER_TRI_OUT_SRC_CH0); + timer_channel_output_pulse_value_config(TIMER1, TIMER_CH_1, 0); + timer_channel_output_mode_config(TIMER1, TIMER_CH_1, TIMER_OC_MODE_PWM0); + timer_channel_output_shadow_config(TIMER1, TIMER_CH_1, TIMER_OC_SHADOW_DISABLE); + // IRQ + timer_interrupt_enable(TIMER1, TIMER_INT_UP); + timer_interrupt_enable(TIMER1, TIMER_INT_CH1); + } + + eclic_irq_enable(TIMER1_IRQn, 2, 5); + // Setup timer 2 to control the output signal + { + timer_deinit(TIMER2); + /* initialize TIMER init parameter struct */ + timer_struct_para_init(&timer_initpara); + /* TIMER1 configuration */ + timer_initpara.prescaler = 200; + timer_initpara.alignedmode = TIMER_COUNTER_EDGE; + timer_initpara.counterdirection = TIMER_COUNTER_UP; + timer_initpara.period = 100; + timer_initpara.clockdivision = TIMER_CKDIV_DIV4; + timer_initpara.repetitioncounter = 0; + timer_init(TIMER2, &timer_initpara); + + /* CH0 configuration in PWM mode0 */ + timer_channel_output_struct_para_init(&timer_ocintpara); + timer_ocintpara.outputstate = TIMER_CCX_ENABLE; + timer_ocintpara.outputnstate = TIMER_CCXN_DISABLE; + timer_ocintpara.ocpolarity = TIMER_OC_POLARITY_HIGH; + timer_ocintpara.ocnpolarity = TIMER_OCN_POLARITY_HIGH; + timer_ocintpara.ocidlestate = TIMER_OC_IDLE_STATE_LOW; + timer_ocintpara.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW; + timer_channel_output_config(TIMER2, TIMER_CH_0, &timer_ocintpara); + timer_channel_output_pulse_value_config(TIMER2, TIMER_CH_0, 0); + timer_channel_output_mode_config(TIMER2, TIMER_CH_0, TIMER_OC_MODE_PWM0); + timer_channel_output_shadow_config(TIMER2, TIMER_CH_0, TIMER_OC_SHADOW_DISABLE); + timer_auto_reload_shadow_enable(TIMER2); + timer_enable(TIMER2); + } +} +void setup_iwdg() { + + fwdgt_config(0x0FFF, FWDGT_PSC_DIV256); + fwdgt_enable(); +} + +void setupFUSBIRQ() { + eclic_global_interrupt_enable(); + eclic_irq_enable(EXTI5_9_IRQn, 15, 0); + gpio_exti_source_select(GPIO_PORT_SOURCE_GPIOB, GPIO_PIN_SOURCE_5); + + /* configure key EXTI line */ + exti_init(EXTI_5, EXTI_INTERRUPT, EXTI_TRIG_FALLING); +} diff --git a/source/Core/BSP/Magic/Setup.h b/source/Core/BSP/Magic/Setup.h new file mode 100644 index 00000000..d95913e2 --- /dev/null +++ b/source/Core/BSP/Magic/Setup.h @@ -0,0 +1,26 @@ +/* + * Setup.h + * + * Created on: 29Aug.,2017 + * Author: Ben V. Brown + */ + +#ifndef PINE_SETUP_H_ +#define PINE_SETUP_H_ +#include "gd32vf103_libopt.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif +uint16_t getADC(uint8_t channel); +void hardware_init(); +uint16_t getADCHandleTemp(uint8_t sample); +uint16_t getADCVin(uint8_t sample); +#ifdef __cplusplus +} +#endif +void setupFUSBIRQ(); +extern const uint8_t holdoffTicks; +extern const uint8_t tempMeasureTicks; +#endif /* PINE_SETUP_H_ */ diff --git a/source/Core/BSP/Magic/ThermoModel.cpp b/source/Core/BSP/Magic/ThermoModel.cpp new file mode 100644 index 00000000..1f4da291 --- /dev/null +++ b/source/Core/BSP/Magic/ThermoModel.cpp @@ -0,0 +1,72 @@ +/* + * ThermoModel.cpp + * + * Created on: 1 May 2021 + * Author: Ralim + */ +#include "TipThermoModel.h" +#include "Utils.h" +#include "configuration.h" + +#ifdef TEMP_uV_LOOKUP_HAKKO +const uint16_t uVtoDegC[] = { + // + // uv -> temp in C + 0, 0, // + 266, 10, // + 522, 20, // + 770, 30, // + 1010, 40, // + 1244, 50, // + 1473, 60, // + 1697, 70, // + 1917, 80, // + 2135, 90, // + 2351, 100, // + 2566, 110, // + 2780, 120, // + 2994, 130, // + 3209, 140, // + 3426, 150, // + 3644, 160, // + 3865, 170, // + 4088, 180, // + 4314, 190, // + 4544, 200, // + 4777, 210, // + 5014, 220, // + 5255, 230, // + 5500, 240, // + 5750, 250, // + 6003, 260, // + 6261, 270, // + 6523, 280, // + 6789, 290, // + 7059, 300, // + 7332, 310, // + 7609, 320, // + 7889, 330, // + 8171, 340, // + 8456, 350, // + 8742, 360, // + 9030, 370, // + 9319, 380, // + 9607, 390, // + 9896, 400, // + 10183, 410, // + 10468, 420, // + 10750, 430, // + 11029, 440, // + 11304, 450, // + 11573, 460, // + 11835, 470, // + 12091, 480, // + 12337, 490, // + 12575, 500, // + +}; +#endif + +const int uVtoDegCItems = sizeof(uVtoDegC) / (2 * sizeof(uint16_t)); + +uint32_t TipThermoModel::convertuVToDegC(uint32_t tipuVDelta) { return Utils::InterpolateLookupTable(uVtoDegC, uVtoDegCItems, tipuVDelta); } diff --git a/source/Core/BSP/Magic/UnitSettings.h b/source/Core/BSP/Magic/UnitSettings.h new file mode 100644 index 00000000..20fadb8b --- /dev/null +++ b/source/Core/BSP/Magic/UnitSettings.h @@ -0,0 +1,11 @@ +/* + * UnitSettings.h + * + * Created on: 29 May 2020 + * Author: Ralim + */ + +#ifndef BSP_PINE64_UNITSETTINGS_H_ +#define BSP_PINE64_UNITSETTINGS_H_ + +#endif /* BSP_PINE64_UNITSETTINGS_H_ */ diff --git a/source/Core/BSP/Magic/bl_mcu_sdk b/source/Core/BSP/Magic/bl_mcu_sdk new file mode 160000 index 00000000..8742503a --- /dev/null +++ b/source/Core/BSP/Magic/bl_mcu_sdk @@ -0,0 +1 @@ +Subproject commit 8742503a9e9aaa36462767b300f3c22b2935cc5e diff --git a/source/Core/BSP/Magic/configuration.h b/source/Core/BSP/Magic/configuration.h new file mode 100644 index 00000000..9fc1959b --- /dev/null +++ b/source/Core/BSP/Magic/configuration.h @@ -0,0 +1,155 @@ +#ifndef CONFIGURATION_H_ +#define CONFIGURATION_H_ +#include "Settings.h" +#include +/** + * Configuration.h + * Define here your default pre settings for Pinecil + * + */ + +//=========================================================================== +//============================= Default Settings ============================ +//=========================================================================== +/** + * Default soldering temp is 320.0 C + * Temperature the iron sleeps at - default 150.0 C + */ + +#define SLEEP_TEMP 150 // Default sleep temperature +#define BOOST_TEMP 420 // Default boost temp. +#define BOOST_MODE_ENABLED 1 // 0: Disable 1: Enable + +/** + * Blink the temperature on the cooling screen when its > 50C + */ +#define COOLING_TEMP_BLINK 0 // 0: Disable 1: Enable + +/** + * How many seconds/minutes we wait until going to sleep/shutdown. + * Values -> SLEEP_TIME * 10; i.e. 5*10 = 50 Seconds! + */ +#define SLEEP_TIME 5 // x10 Seconds +#define SHUTDOWN_TIME 10 // Minutes + +/** + * Auto start off for safety. + * Pissible values are: + * 0 - none + * 1 - Soldering Temperature + * 2 - Sleep Temperature + * 3 - Sleep Off Temperature + */ +#define AUTO_START_MODE 0 // Default to none + +/** + * Locking Mode + * When in soldering mode a long press on both keys toggle the lock of the buttons + * Possible values are: + * 0 - Desactivated + * 1 - Lock except boost + * 2 - Full lock + */ +#define LOCKING_MODE 0 // Default to desactivated for safety + +/** + * OLED Orientation + * + */ +#define ORIENTATION_MODE 2 // 0: Right 1:Left 2:Automatic - Default Automatic +#define REVERSE_BUTTON_TEMP_CHANGE 0 // 0:Default 1:Reverse - Reverse the plus and minus button assigment for temperature change + +/** + * Temp change settings + */ +#define TEMP_CHANGE_SHORT_STEP 1 // Default temp change short step +1 +#define TEMP_CHANGE_LONG_STEP 10 // Default temp change long step +10 +#define TEMP_CHANGE_SHORT_STEP_MAX 50 // Temp change short step MAX value +#define TEMP_CHANGE_LONG_STEP_MAX 90 // Temp change long step MAX value + +/* Power pulse for keeping power banks awake*/ +#define POWER_PULSE_INCREMENT 1 +#define POWER_PULSE_MAX 100 // x10 max watts +#define POWER_PULSE_WAIT_MAX 9 // 9*2.5s = 22.5 seconds +#define POWER_PULSE_DURATION_MAX 9 // 9*250ms = 2.25 seconds + +#ifdef MODEL_Pinecil +#define POWER_PULSE_DEFAULT 0 +#else +#define POWER_PULSE_DEFAULT 5 +#endif +#define POWER_PULSE_WAIT_DEFAULT 4 // Default rate of the power pulse: 4*2500 = 10000 ms = 10 s +#define POWER_PULSE_DURATION_DEFAULT 1 // Default duration of the power pulse: 1*250 = 250 ms + +/** + * OLED Orientation Sensitivity on Automatic mode! + * Motion Sensitivity <0=Off 1=Least Sensitive 9=Most Sensitive> + */ +#define SENSITIVITY 7 // Default 7 + +/** + * Detailed soldering screen + * Detailed idle screen (off for first time users) + */ +#define DETAILED_SOLDERING 0 // 0: Disable 1: Enable - Default 0 +#define DETAILED_IDLE 0 // 0: Disable 1: Enable - Default 0 + +#define THERMAL_RUNAWAY_TIME_SEC 20 +#define THERMAL_RUNAWAY_TEMP_C 20 + +#define CUT_OUT_SETTING 0 // default to no cut-off voltage +#define RECOM_VOL_CELL 33 // Minimum voltage per cell (Recommended 3.3V (33)) +#define TEMPERATURE_INF 0 // default to 0 +#define DESCRIPTION_SCROLL_SPEED 0 // 0: Slow 1: Fast - default to slow +#define ANIMATION_LOOP 1 // 0: off 1: on +#define ANIMATION_SPEED settingOffSpeed_t::MEDIUM + +#define OP_AMP_Rf_Pinecil 750 * 1000 // 750 Kilo-ohms -> From schematic, R1 +#define OP_AMP_Rin_Pinecil 2370 // 2.37 Kilo-ohms -> From schematic, R2 + +#define OP_AMP_GAIN_STAGE_PINECIL (1 + (OP_AMP_Rf_Pinecil / OP_AMP_Rin_Pinecil)) + +#if defined(MODEL_Pinecil) == 0 +#error "No model defined!" +#endif + +#ifdef MODEL_Pinecil +#define SOLDERING_TEMP 320 // Default soldering temp is 320.0 °C +#define VOLTAGE_DIV 467 // 467 - Default divider from schematic +#define CALIBRATION_OFFSET 900 // 900 - Default adc offset in uV +#define MIN_CALIBRATION_OFFSET 100 // Min value for calibration +#define PID_POWER_LIMIT 70 // Sets the max pwm power limit +#define POWER_LIMIT 0 // 0 watts default limit +#define MAX_POWER_LIMIT 70 // +#define POWER_LIMIT_STEPS 5 // +#define OP_AMP_GAIN_STAGE OP_AMP_GAIN_STAGE_PINECIL // Uses TS100 resistors +#define TEMP_uV_LOOKUP_HAKKO // Use Hakko lookup table +#define USB_PD_VMAX 20 // Maximum voltage for PD to negotiate +#define PID_TIM_HZ (8) // Tick rate of the PID loop +#define MAX_TEMP_C 450 // Max soldering temp selectable °C +#define MAX_TEMP_F 850 // Max soldering temp selectable °F +#define MIN_TEMP_C 10 // Min soldering temp selectable °C +#define MIN_TEMP_F 60 // Min soldering temp selectable °F +#define MIN_BOOST_TEMP_C 250 // The min settable temp for boost mode °C +#define MIN_BOOST_TEMP_F 480 // The min settable temp for boost mode °F + +#define POW_PD 1 +#define POW_QC 1 +#define POW_DC 1 +#define POW_QC_20V 1 +#define ENABLE_QC2 1 +#define TEMP_TMP36 +#define ACCEL_BMA +#define ACCEL_SC7 +#define HALL_SENSOR +#define VBUS_MOD_TEST +#define HALL_SI7210 +#define DEBUG_UART_OUTPUT + +#define HARDWARE_MAX_WATTAGE_X10 750 +#define TIP_THERMAL_MASS 65 // X10 watts to raise 1 deg C in 1 second +#define TIP_RESISTANCE 75 // x10 ohms, 7.5 typical for Pinecil tips +#endif +#endif + +#define FLASH_LOGOADDR (0x08000000 + (126 * 1024)) diff --git a/source/Core/BSP/Magic/flash.c b/source/Core/BSP/Magic/flash.c new file mode 100644 index 00000000..8c0ce3f5 --- /dev/null +++ b/source/Core/BSP/Magic/flash.c @@ -0,0 +1,46 @@ +/* + * flash.c + * + * Created on: 29 May 2020 + * Author: Ralim + */ + +#include "BSP.h" +#include "BSP_Flash.h" +#include "gd32vf103_libopt.h" +#include "string.h" +#define FMC_PAGE_SIZE ((uint16_t)0x400U) +// static uint16_t settings_page[FMC_PAGE_SIZE] __attribute__ ((section (".settings_page"))); +// Linker script doesnt want to play, so for now its hard coded +#define SETTINGS_START_PAGE (0x08000000 + (127 * 1024)) +uint8_t flash_save_buffer(const uint8_t *buffer, const uint16_t length) { + + /* unlock the flash program/erase controller */ + fmc_unlock(); + + /* clear all pending flags */ + fmc_flag_clear(FMC_FLAG_END); + fmc_flag_clear(FMC_FLAG_WPERR); + fmc_flag_clear(FMC_FLAG_PGERR); + resetWatchdog(); + fmc_page_erase((uint32_t)SETTINGS_START_PAGE); + resetWatchdog(); + uint16_t *data = (uint16_t *)buffer; + for (uint16_t i = 0; i < (length / 2); i++) { + fmc_halfword_program((uint32_t)SETTINGS_START_PAGE + (i * 2), data[i]); + fmc_flag_clear(FMC_FLAG_END); + fmc_flag_clear(FMC_FLAG_WPERR); + fmc_flag_clear(FMC_FLAG_PGERR); + resetWatchdog(); + } + fmc_lock(); + return 1; +} + +void flash_read_buffer(uint8_t *buffer, const uint16_t length) { + uint32_t *b = (uint32_t *)buffer; + uint32_t *b2 = (uint32_t *)SETTINGS_START_PAGE; + for (int i = 0; i < length / 4; i++) { + b[i] = b2[i]; + } +} diff --git a/source/Core/BSP/Magic/fusb_user.cpp b/source/Core/BSP/Magic/fusb_user.cpp new file mode 100644 index 00000000..0e06e943 --- /dev/null +++ b/source/Core/BSP/Magic/fusb_user.cpp @@ -0,0 +1,27 @@ +#include "configuration.h" +#if POW_PD +#include "BSP.h" +#include "I2C_Wrapper.hpp" +#include "Setup.h" + +/* + * Read multiple bytes from the FUSB302B + * + * cfg: The FUSB302B to communicate with + * addr: The memory address from which to read + * size: The number of bytes to read + * buf: The buffer into which data will be read + */ +bool fusb_read_buf(const uint8_t deviceAddr, const uint8_t registerAdd, const uint8_t size, uint8_t *buf) { return FRToSI2C::Mem_Read(deviceAddr, registerAdd, buf, size); } + +/* + * Write multiple bytes to the FUSB302B + * + * cfg: The FUSB302B to communicate with + * addr: The memory address to which we will write + * size: The number of bytes to write + * buf: The buffer to write + */ +bool fusb_write_buf(const uint8_t deviceAddr, const uint8_t registerAdd, const uint8_t size, uint8_t *buf) { return FRToSI2C::Mem_Write(deviceAddr, registerAdd, (uint8_t *)buf, size); } + +#endif diff --git a/source/Core/BSP/Magic/postRTOS.cpp b/source/Core/BSP/Magic/postRTOS.cpp new file mode 100644 index 00000000..7e10c24e --- /dev/null +++ b/source/Core/BSP/Magic/postRTOS.cpp @@ -0,0 +1,29 @@ +#include "BSP.h" +#include "FreeRTOS.h" +#include "I2C_Wrapper.hpp" +#include "QC3.h" +#include "Settings.h" +#include "Si7210.h" +#include "cmsis_os.h" +#include "main.hpp" +#include "power.hpp" +#include "stdlib.h" +#include "task.h" + +bool hall_effect_present = false; +void postRToSInit() { + // Any after RTos setup +#ifdef HALL_SI7210 + if (Si7210::detect()) { + hall_effect_present = Si7210::init(); + } +#endif +} +int16_t getRawHallEffect() { + if (hall_effect_present) { + return Si7210::read(); + } + return 0; +} + +bool getHallSensorFitted() { return hall_effect_present; } diff --git a/source/Core/BSP/Magic/preRTOS.cpp b/source/Core/BSP/Magic/preRTOS.cpp new file mode 100644 index 00000000..4a013b80 --- /dev/null +++ b/source/Core/BSP/Magic/preRTOS.cpp @@ -0,0 +1,21 @@ +/* + * preRTOS.c + * + * Created on: 29 May 2020 + * Author: Ralim + */ + +#include "BSP.h" +#include "Pins.h" +#include "Setup.h" +#include "gd32vf103_libopt.h" +#include +void preRToSInit() { + // Normal system bringup -- GPIO etc + + hardware_init(); + gpio_bit_reset(OLED_RESET_GPIO_Port, OLED_RESET_Pin); + delay_ms(5); + gpio_bit_set(OLED_RESET_GPIO_Port, OLED_RESET_Pin); + FRToSI2C::FRToSInit(); +}