hoverboard-firmware-hack-se.../Src/control.c

136 lines
3.2 KiB
C
Raw Normal View History

2018-02-09 08:39:47 +00:00
#include "stm32f1xx_hal.h"
#include "defines.h"
#include "setup.h"
#include "config.h"
#include <stdbool.h>
#include <string.h>
2018-02-09 08:39:47 +00:00
TIM_HandleTypeDef TimHandle;
uint8_t ppm_count = 0;
uint32_t timeout = 100;
2018-04-07 22:03:35 +00:00
uint8_t nunchuck_data[6] = {0};
2018-05-06 18:11:53 +00:00
uint8_t i2cBuffer[2];
2018-04-07 22:03:35 +00:00
extern I2C_HandleTypeDef hi2c2;
DMA_HandleTypeDef hdma_i2c2_rx;
DMA_HandleTypeDef hdma_i2c2_tx;
2018-02-09 08:39:47 +00:00
typedef struct{
float motorR;
float motorL;
//uint32_t crc;
} Serialcommand;
#ifdef CONTROL_PPM
uint16_t ppm_captured_value[PPM_NUM_CHANNELS + 1] = {500, 500};
uint16_t ppm_captured_value_buffer[PPM_NUM_CHANNELS+1] = {500, 500};
uint32_t ppm_timeout = 0;
bool ppm_valid = true;
volatile Serialcommand command;
#define IN_RANGE(x, low, up) (((x) >= (low)) && ((x) <= (up)))
#ifdef CONTROL_SERIAL_USART2
void Telemetry_Init(){
HAL_UART_Receive_DMA(&huart2, (uint8_t *)&command, 8);
}
#endif
2018-02-09 08:39:47 +00:00
void PPM_ISR_Callback() {
// Dummy loop with 16 bit count wrap around
uint16_t rc_delay = TIM2->CNT;
TIM2->CNT = 0;
if (rc_delay > 3000) {
if (ppm_valid && ppm_count == PPM_NUM_CHANNELS) {
ppm_timeout = 0;
memcpy(ppm_captured_value, ppm_captured_value_buffer, sizeof(ppm_captured_value));
}
ppm_valid = true;
2018-02-09 08:39:47 +00:00
ppm_count = 0;
}
else if (ppm_count < PPM_NUM_CHANNELS && IN_RANGE(rc_delay, 900, 2100)){
2018-02-09 08:39:47 +00:00
timeout = 0;
ppm_captured_value_buffer[ppm_count++] = CLAMP(rc_delay, 1000, 2000) - 1000;
} else {
ppm_valid = false;
2018-02-09 08:39:47 +00:00
}
}
// SysTick executes once each ms
void PPM_SysTick_Callback() {
ppm_timeout++;
// Stop after 500 ms without PPM signal
if(ppm_timeout > 500) {
int i;
for(i = 0; i < PPM_NUM_CHANNELS; i++) {
ppm_captured_value[i] = 500;
}
ppm_timeout = 0;
}
2018-02-09 08:39:47 +00:00
}
void PPM_Init() {
GPIO_InitTypeDef GPIO_InitStruct;
/*Configure GPIO pin : PA3 */
GPIO_InitStruct.Pin = GPIO_PIN_3;
GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
__HAL_RCC_TIM2_CLK_ENABLE();
TimHandle.Instance = TIM2;
TimHandle.Init.Period = UINT16_MAX;
TimHandle.Init.Prescaler = (SystemCoreClock/DELAY_TIM_FREQUENCY_US)-1;;
TimHandle.Init.ClockDivision = 0;
TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
HAL_TIM_Base_Init(&TimHandle);
/* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI3_IRQn);
HAL_TIM_Base_Start(&TimHandle);
2018-02-09 08:39:47 +00:00
}
#endif
2018-04-07 22:03:35 +00:00
void Nunchuck_Init() {
//-- START -- init WiiNunchuck
2018-05-06 18:11:53 +00:00
i2cBuffer[0] = 0xF0;
i2cBuffer[1] = 0x55;
HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 2, 100);
HAL_Delay(10);
i2cBuffer[0] = 0xFB;
i2cBuffer[1] = 0x00;
HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 2, 100);
2018-04-07 22:03:35 +00:00
HAL_Delay(10);
}
void Nunchuck_Read() {
2018-05-06 18:11:53 +00:00
i2cBuffer[0] = 0x00;
HAL_I2C_Master_Transmit(&hi2c2,0xA4,(uint8_t*)i2cBuffer, 1, 100);
HAL_Delay(5);
2018-05-06 18:11:53 +00:00
if (HAL_I2C_Master_Receive(&hi2c2,0xA4,(uint8_t*)nunchuck_data, 6, 100) == HAL_OK) {
timeout = 0;
} else {
timeout++;
2018-04-07 22:03:35 +00:00
}
2018-05-06 18:11:53 +00:00
if (timeout > 3) {
HAL_Delay(50);
Nunchuck_Init();
}
2018-04-07 22:03:35 +00:00
//setScopeChannel(0, (int)nunchuck_data[0]);
//setScopeChannel(1, (int)nunchuck_data[1]);
//setScopeChannel(2, (int)nunchuck_data[5] & 1);
//setScopeChannel(3, ((int)nunchuck_data[5] >> 1) & 1);
}