*
This commit is contained in:
@@ -0,0 +1,251 @@
|
||||
/*
|
||||
* copter_main.cpp
|
||||
*
|
||||
* Created on: 02 ÿíâ. 2016 ã.
|
||||
* Author: Sergey
|
||||
*/
|
||||
|
||||
#include "stm32f1xx_hal.h"
|
||||
#include "uart_protocol.h"
|
||||
#include "uart_commands.h"
|
||||
#include "motor.h"
|
||||
#include "sound_module.h"
|
||||
#include "nrf_stm32.h"
|
||||
#include "radio_protocol.h"
|
||||
//#include "bma_accelerometer.h"
|
||||
|
||||
extern TIM_HandleTypeDef htim1;
|
||||
extern UART_HandleTypeDef huart3;
|
||||
extern SPI_HandleTypeDef hspi2;
|
||||
/*
|
||||
bma_accelerometer<&hspi2> accelerometer;
|
||||
|
||||
UartCommand rx_cmd0 __attribute__ ((aligned (4)));
|
||||
UartCommand rx_cmd1 __attribute__ ((aligned (4)));
|
||||
UartCommand* rx_cmds[] = {&rx_cmd0, &rx_cmd1};
|
||||
|
||||
|
||||
SoundNode armed_sound[6] = {
|
||||
{SoundNodeType::Wave, Notes::C() << 4, SOUND_MS_TO_SAMPLES(150)},
|
||||
{SoundNodeType::Silence, 1, SOUND_MS_TO_SAMPLES(55)},
|
||||
{SoundNodeType::Wave, Notes::F_d() << 4, SOUND_MS_TO_SAMPLES(150)},
|
||||
{SoundNodeType::Silence, 1, SOUND_MS_TO_SAMPLES(55)},
|
||||
{SoundNodeType::Wave, Notes::B() << 4, SOUND_MS_TO_SAMPLES(150)},
|
||||
{SoundNodeType::Silence, 1, SOUND_MS_TO_SAMPLES(355)}
|
||||
};
|
||||
|
||||
|
||||
dac_sound_player player;*/
|
||||
|
||||
/*
|
||||
uint32_t active_cmd = 0;
|
||||
volatile UartCommand* received_command = nullptr;
|
||||
|
||||
void begin_cmd_rx(){
|
||||
if(HAL_OK != HAL_UART_Receive_DMA(&huart3,(uint8_t*)rx_cmds[active_cmd], sizeof(UartCommand))){
|
||||
for(;;);
|
||||
}
|
||||
}
|
||||
extern "C" void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart){
|
||||
received_command = rx_cmds[active_cmd];
|
||||
active_cmd = active_cmd == 0 ? 1 : 0;
|
||||
begin_cmd_rx();
|
||||
}
|
||||
|
||||
|
||||
UartCommand* try_get_command(){
|
||||
auto copy = received_command;
|
||||
if(copy != nullptr){
|
||||
received_command = nullptr;
|
||||
}
|
||||
return (UartCommand*)copy;
|
||||
}
|
||||
|
||||
Motor motors[4];
|
||||
|
||||
void execute_command(UartCommand* cmd){
|
||||
UartCommandReader reader(cmd);
|
||||
|
||||
switch(cmd->header.id){
|
||||
case uart_commands::led_read:{
|
||||
auto led_id = reader.read<uint8_t>();
|
||||
bool state = false;
|
||||
if(led_id == 0) {
|
||||
//state = HAL_GPIO_ReadPin(LED_BLUE_GPIO_Port, LED_BLUE_Pin) == GPIO_PIN_SET;
|
||||
}else{
|
||||
state = HAL_GPIO_ReadPin(LED_GREEN_GPIO_Port, LED_GREEN_Pin) == GPIO_PIN_SET;
|
||||
}
|
||||
|
||||
UartCommandWriter writer(cmd);
|
||||
writer.write(led_id).write(state);
|
||||
HAL_UART_Transmit(&huart3, (uint8_t*)cmd, sizeof(*cmd), HAL_MAX_DELAY);
|
||||
}
|
||||
break;
|
||||
case uart_commands::led_write:{
|
||||
auto led_id = reader.read<uint8_t>();
|
||||
auto led_state = reader.read<bool>();
|
||||
if(led_id == 0) {
|
||||
//HAL_GPIO_WritePin(LED_BLUE_GPIO_Port, LED_BLUE_Pin, led_state ? GPIO_PIN_SET : GPIO_PIN_RESET);
|
||||
}else{
|
||||
HAL_GPIO_WritePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin, led_state ? GPIO_PIN_SET : GPIO_PIN_RESET);
|
||||
}
|
||||
HAL_UART_Transmit(&huart3, (uint8_t*)cmd, sizeof(*cmd), HAL_MAX_DELAY);
|
||||
}
|
||||
break;
|
||||
case uart_commands::motor_write:{
|
||||
auto motor_id = reader.read<uint8_t>();
|
||||
auto motor_speed = reader.read<uint8_t>();
|
||||
motors[motor_id].set_speed(motor_speed);
|
||||
HAL_UART_Transmit(&huart3, (uint8_t*)cmd, sizeof(*cmd), HAL_MAX_DELAY);
|
||||
}
|
||||
break;
|
||||
case uart_commands::play_sound:{
|
||||
player.play_sound(armed_sound);
|
||||
HAL_UART_Transmit(&huart3, (uint8_t*)cmd, sizeof(*cmd), HAL_MAX_DELAY);
|
||||
}
|
||||
break;
|
||||
case uart_commands::acc_read:{
|
||||
auto acc = accelerometer.get_acceleration();
|
||||
UartCommandWriter writer(cmd);
|
||||
writer.write(acc);
|
||||
HAL_UART_Transmit(&huart3, (uint8_t*)cmd, sizeof(*cmd), HAL_MAX_DELAY);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
while(true){
|
||||
HAL_GPIO_TogglePin(LED_GREEN_GPIO_Port, LED_GREEN_Pin);
|
||||
HAL_Delay(500);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}*/
|
||||
|
||||
radio_t radio;
|
||||
|
||||
uint32_t rx_size = 0;
|
||||
uint8_t rx_buffer[32];
|
||||
uint8_t tx_buffer[32];
|
||||
|
||||
uint64_t rx_size_total = 0;
|
||||
|
||||
uint8_t ack_pipe_id = 3;
|
||||
|
||||
uint32_t rx_pipe_counters[] = {0,0,0,0,0,0,0};
|
||||
uint32_t rx_idle_counter = 0;
|
||||
|
||||
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin){
|
||||
if(GPIO_Pin == RADIO_IRQ_Pin){
|
||||
auto status = radio.clear_irq_flags_get_status();
|
||||
|
||||
if(status.rx_dr == 0){
|
||||
printf("No data in RX radio! Something is really wrong!\r\n");
|
||||
while(true);
|
||||
}else{
|
||||
if(status.rx_p_no != 7){
|
||||
do{
|
||||
rx_size = radio.read_rx_payload(rx_buffer);
|
||||
rx_size_total += rx_size;
|
||||
|
||||
RadioPacketHeader hdr;
|
||||
hdr.value = rx_buffer[0];
|
||||
|
||||
if(hdr.is_idle){
|
||||
rx_idle_counter++;
|
||||
}else{
|
||||
rx_pipe_counters[hdr.pipe_id]++;
|
||||
}
|
||||
|
||||
|
||||
}while(!radio.rx_fifo_empty());
|
||||
|
||||
while(!radio.tx_fifo_full()){
|
||||
RadioPacketHeader hdr;
|
||||
hdr.is_idle = 0;
|
||||
hdr.pipe_id = ack_pipe_id;
|
||||
hdr.reserved = 0;
|
||||
|
||||
tx_buffer[0] = hdr.value;
|
||||
ack_pipe_id = ack_pipe_id == 3 ? 1 : 3;
|
||||
radio.write_ack_payload(0, tx_buffer, 15);
|
||||
}
|
||||
}else{
|
||||
radio.flush_rx();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
extern "C" void copter_main(){
|
||||
setvbuf(stdout, NULL, _IONBF, 0);
|
||||
printf("Hello!\r\n");
|
||||
|
||||
radio.init();
|
||||
|
||||
radio.set_irq_mode(irq_source_t::max_rt, false);
|
||||
radio.set_irq_mode(irq_source_t::rx_dr, true);
|
||||
radio.set_irq_mode(irq_source_t::tx_ds, false);
|
||||
radio.open_pipe(pipe_id::pipe0, true);
|
||||
radio.set_auto_retr(15, 250);
|
||||
radio.enable_dynamic_payload(true);
|
||||
radio.enable_dynamic_ack(true);
|
||||
radio.enable_ack_payload(true);
|
||||
radio.set_address_width(nrf24::address_width_t::aw_3bytes);
|
||||
radio.set_datarate(nrf24::datarate_t::mbps_2);
|
||||
radio.set_mode(Mode::rx);
|
||||
|
||||
|
||||
uint8_t addr_p0[5];
|
||||
radio.read_multibyte_reg((uint8_t)pipe_id::pipe0, &addr_p0[0]);
|
||||
printf("rx_addr_p0: 0x%02x%02x%02x%02x%02x\r\n", (int)addr_p0[4], (int)addr_p0[3], (int)addr_p0[2], (int)addr_p0[1], (int)addr_p0[0]);
|
||||
|
||||
uint32_t old_rx = 0;
|
||||
while(true){
|
||||
HAL_Delay(1000);
|
||||
HAL_GPIO_TogglePin(LED_BLUE_GPIO_Port, LED_BLUE_Pin);
|
||||
|
||||
printf("RX speed[byte/s]: %i, p0:%i p1:%i p2:%i p3:%i p4:%i idle:%i\r\n", (int)(rx_size_total - old_rx),
|
||||
(int)rx_pipe_counters[0],
|
||||
(int)rx_pipe_counters[1],
|
||||
(int)rx_pipe_counters[2],
|
||||
(int)rx_pipe_counters[3],
|
||||
(int)rx_pipe_counters[4], (int)rx_idle_counter);
|
||||
|
||||
old_rx = rx_size_total;
|
||||
}
|
||||
|
||||
|
||||
/*motors[0].init(&htim1, TIM_CHANNEL_1);
|
||||
motors[0].set_speed(0);
|
||||
|
||||
motors[1].init(&htim1, TIM_CHANNEL_2);
|
||||
motors[1].set_speed(0);
|
||||
|
||||
motors[2].init(&htim1, TIM_CHANNEL_3);
|
||||
motors[2].set_speed(0);
|
||||
|
||||
motors[3].init(&htim1, TIM_CHANNEL_4);
|
||||
motors[3].set_speed(0);
|
||||
|
||||
|
||||
HAL_GPIO_WritePin(SS_GYRO_GPIO_Port, SS_GYRO_Pin, GPIO_PIN_SET);
|
||||
HAL_GPIO_WritePin(SS_ACC_GPIO_Port, SS_ACC_Pin, GPIO_PIN_SET);
|
||||
|
||||
accelerometer.init();
|
||||
|
||||
if(!accelerometer.set_power(true)){
|
||||
for(;;);
|
||||
}
|
||||
|
||||
player.play_sound(armed_sound);
|
||||
|
||||
begin_cmd_rx();
|
||||
while(true){
|
||||
UartCommand* cmd = nullptr;
|
||||
|
||||
//wait command
|
||||
while((cmd = try_get_command()) == nullptr){}
|
||||
|
||||
execute_command(cmd);
|
||||
}*/
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
/*
|
||||
* motor.cpp
|
||||
*
|
||||
* Created on: 08 íîÿá. 2015 ã.
|
||||
* Author: Sergey
|
||||
*/
|
||||
|
||||
#include "motor.h"
|
||||
#include <algorithm>
|
||||
|
||||
Motor::Motor()
|
||||
:_channel(0), _pwm_timer(nullptr){
|
||||
}
|
||||
|
||||
void Motor::init(TIM_HandleTypeDef* timer, uint32_t channel){
|
||||
_pwm_timer = timer;
|
||||
_channel = channel;
|
||||
}
|
||||
|
||||
|
||||
void Motor::set_speed(uint8_t speed_percent){
|
||||
speed_percent = std::min<uint8_t>(100, speed_percent);
|
||||
auto ccr_value = (_pwm_timer->Instance->ARR * speed_percent) / 100;
|
||||
|
||||
if(speed_percent == 0){
|
||||
HAL_TIM_PWM_Stop(_pwm_timer, _channel);
|
||||
}else{
|
||||
switch (_channel){
|
||||
case TIM_CHANNEL_1:
|
||||
_pwm_timer->Instance->CCR1 = ccr_value;
|
||||
break;
|
||||
case TIM_CHANNEL_2:
|
||||
_pwm_timer->Instance->CCR2 = ccr_value;
|
||||
break;
|
||||
case TIM_CHANNEL_3:
|
||||
_pwm_timer->Instance->CCR3 = ccr_value;
|
||||
break;
|
||||
case TIM_CHANNEL_4:
|
||||
_pwm_timer->Instance->CCR4 = ccr_value;
|
||||
break;
|
||||
}
|
||||
|
||||
HAL_TIM_PWM_Start(_pwm_timer, _channel);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
/*
|
||||
* motor.h
|
||||
*
|
||||
* Created on: 08 íîÿá. 2015 ã.
|
||||
* Author: Sergey
|
||||
*/
|
||||
|
||||
#ifndef SRC_MOTOR_H_
|
||||
#define SRC_MOTOR_H_
|
||||
|
||||
#include <cstdint>
|
||||
#include "stm32f1xx_hal.h"
|
||||
|
||||
class Motor {
|
||||
public:
|
||||
Motor();
|
||||
void init(TIM_HandleTypeDef* timer, uint32_t channel);
|
||||
void set_speed(uint8_t speed_percent);
|
||||
private:
|
||||
uint32_t _channel;
|
||||
TIM_HandleTypeDef* _pwm_timer;
|
||||
};
|
||||
|
||||
|
||||
#endif /* SRC_MOTOR_H_ */
|
||||
@@ -0,0 +1,51 @@
|
||||
/*
|
||||
* sound.cpp
|
||||
*
|
||||
* Created on: 03 ÿíâ. 2016 ã.
|
||||
* Author: Sergey
|
||||
*/
|
||||
|
||||
|
||||
#include "sound.h"
|
||||
|
||||
/*//16-bit sine table (0..Pi/2)
|
||||
const int16_t sine_tab[64] = {
|
||||
0, 804, 1607, 2410, 3211, 4011, 4807, 5601, 6392, 7179,
|
||||
7961, 8739, 9511, 10278,11039,11792,12539,13278,14009,14732,
|
||||
15446,16151,16845,17530,18204,18867,19519,20159,20787,21402,
|
||||
22005,22594,23170,23731,24279,24811,25329,25832,26319,26790,
|
||||
27245,27683,28105,28510,28898,29268,29621,29956,30273,30571,
|
||||
30852,31113,31356,31580,31785,31971,32137,32285,32412,32521,
|
||||
32609,32678,32728,32757
|
||||
};
|
||||
*/
|
||||
|
||||
|
||||
/*int16_t get_sine(uint8_t phase8){
|
||||
auto result = (phase8 & 0x40) ? sine_tab[63 - (phase8 & 0x3F)] : sine_tab[phase8 & 0x3F];
|
||||
if (phase8 & 0x80) result = -result;
|
||||
return result;
|
||||
}*/
|
||||
|
||||
const unsigned char sinetable[256] = {
|
||||
128,131,134,137,140,143,146,149,152,156,159,162,165,168,171,174,
|
||||
176,179,182,185,188,191,193,196,199,201,204,206,209,211,213,216,
|
||||
218,220,222,224,226,228,230,232,234,236,237,239,240,242,243,245,
|
||||
246,247,248,249,250,251,252,252,253,254,254,255,255,255,255,255,
|
||||
255,255,255,255,255,255,254,254,253,252,252,251,250,249,248,247,
|
||||
246,245,243,242,240,239,237,236,234,232,230,228,226,224,222,220,
|
||||
218,216,213,211,209,206,204,201,199,196,193,191,188,185,182,179,
|
||||
176,174,171,168,165,162,159,156,152,149,146,143,140,137,134,131,
|
||||
128,124,121,118,115,112,109,106,103,99, 96, 93, 90, 87, 84, 81,
|
||||
79, 76, 73, 70, 67, 64, 62, 59, 56, 54, 51, 49, 46, 44, 42, 39,
|
||||
37, 35, 33, 31, 29, 27, 25, 23, 21, 19, 18, 16, 15, 13, 12, 10,
|
||||
9, 8, 7, 6, 5, 4, 3, 3, 2, 1, 1, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 1, 1, 2, 3, 3, 4, 5, 6, 7, 8,
|
||||
9, 10, 12, 13, 15, 16, 18, 19, 21, 23, 25, 27, 29, 31, 33, 35,
|
||||
37, 39, 42, 44, 46, 49, 51, 54, 56, 59, 62, 64, 67, 70, 73, 76,
|
||||
79, 81, 84, 87, 90, 93, 96, 99, 103,106,109,112,115,118,121,124
|
||||
};
|
||||
|
||||
uint8_t get_sine(uint8_t phase8) {
|
||||
return sinetable[phase8];
|
||||
}
|
||||
@@ -0,0 +1,120 @@
|
||||
/*
|
||||
* syscalls.c
|
||||
*
|
||||
* Created on: 15 θών 2015 γ.
|
||||
* Author: Sergey
|
||||
*/
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
#include <stm32f1xx_hal.h>
|
||||
#include "stm32f1xx_it.h"
|
||||
#include "board.h"
|
||||
|
||||
extern UART_HandleTypeDef CONSOLE_UART;
|
||||
|
||||
extern int errno;
|
||||
register char * stack_ptr asm("sp");
|
||||
|
||||
caddr_t _sbrk(int incr) {
|
||||
extern char end asm("end");
|
||||
static char *heap_end;
|
||||
char *prev_heap_end;
|
||||
|
||||
if (heap_end == 0)
|
||||
heap_end = &end;
|
||||
|
||||
prev_heap_end = heap_end;
|
||||
if (heap_end + incr > stack_ptr) {
|
||||
errno = ENOMEM;
|
||||
return (caddr_t) -1;
|
||||
}
|
||||
|
||||
heap_end += incr;
|
||||
return (caddr_t) prev_heap_end;
|
||||
}
|
||||
|
||||
|
||||
#ifndef __errno_r
|
||||
#include <sys/reent.h>
|
||||
#define __errno_r(reent) reent->_errno
|
||||
#endif
|
||||
|
||||
|
||||
int _write_r(struct _reent *r, int file, char * ptr, int len){
|
||||
(void)r;
|
||||
(void)file;
|
||||
(void)ptr;
|
||||
|
||||
while (HAL_UART_GetState(&CONSOLE_UART) == HAL_UART_STATE_BUSY_TX ||
|
||||
HAL_UART_GetState(&CONSOLE_UART) == HAL_UART_STATE_BUSY_TX_RX)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
//HAL_UART_Transmit_IT(&CONSOLE_UART, (uint8_t*)ptr, len);
|
||||
HAL_UART_Transmit(&CONSOLE_UART, (uint8_t*)ptr, len, HAL_MAX_DELAY);
|
||||
|
||||
return len;
|
||||
}
|
||||
|
||||
int _read_r(struct _reent *r, int file, char * ptr, int len)
|
||||
{
|
||||
(void)r;
|
||||
(void)file;
|
||||
(void)ptr;
|
||||
(void)len;
|
||||
__errno_r(r) = EINVAL;
|
||||
return -1;
|
||||
}
|
||||
|
||||
int _lseek_r(struct _reent *r, int file, int ptr, int dir){
|
||||
(void)r;
|
||||
(void)file;
|
||||
(void)ptr;
|
||||
(void)dir;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int _close_r(struct _reent *r, int file){
|
||||
(void)r;
|
||||
(void)file;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int _open_r ( struct _reent *ptr, const char *file, int flags, int mode ){
|
||||
int fd = -1;
|
||||
return fd;
|
||||
}
|
||||
|
||||
int _fstat_r(struct _reent *r, int file, struct stat * st){
|
||||
(void)r;
|
||||
(void)file;
|
||||
memset(st, 0, sizeof(*st));
|
||||
st->st_mode = S_IFCHR;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int _isatty_r(struct _reent *r, int fd){
|
||||
(void)r;
|
||||
(void)fd;
|
||||
return 1;
|
||||
}
|
||||
|
||||
int _kill (int a, int b){
|
||||
(void)a;
|
||||
(void)b;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int _getpid(int a){
|
||||
(void)a;
|
||||
return 0;
|
||||
}
|
||||
|
||||
void _exit(int status){
|
||||
for(;;);
|
||||
}
|
||||
Reference in New Issue
Block a user