*

firmware/copter_vldiscovery_board/inc/accelerometer.h

@@ -0,0 +1,21 @@
+/*
+ * accelerometer.h
+ *
+ *  Created on: 08 íîÿá. 2015 ã.
+ *      Author: Sergey
+ */
+
+#ifndef SRC_ACCELEROMETER_H_
+#define SRC_ACCELEROMETER_H_
+
+#include "NMath.h"
+
+using namespace NettleMath;
+
+class Accelerometer  {
+public:
+	virtual float3 get_acceleration() = 0;
+};
+
+
+#endif /* SRC_ACCELEROMETER_H_ */

firmware/copter_vldiscovery_board/inc/bma_accelerometer.h

@@ -0,0 +1,127 @@
+/*
+ * bma_accelerometer.h
+ *
+ *  Created on: 08 íîÿá. 2015 ã.
+ *      Author: Sergey
+ */
+
+#ifndef SRC_BMA_ACCELEROMETER_H_
+#define SRC_BMA_ACCELEROMETER_H_
+
+#include "stm32f1xx_hal.h"
+#include "accelerometer.h"
+extern "C"{
+#include "bma2x2.h"
+}
+
+#define SPI_BUFFER_LEN 10
+
+void swap_bytes(s16* value){
+	s8* bytes = (s8*)value;
+	auto tmp = bytes[0];
+	bytes[0] = bytes[1];
+	bytes[1] = tmp;
+}
+
+template<SPI_HandleTypeDef* spi>
+class bma_accelerometer : public Accelerometer {
+public:
+	bma_accelerometer(){
+		BMA_bind_SPI(&_bma2x2);
+	}
+	bool init(){
+		auto result = bma2x2_init(&_bma2x2);
+		result += bma2x2_set_bw(BMA2x2_BW_62_50HZ);
+		return result == 0;
+	}
+	bool set_power(bool on) {
+		return 0 == bma2x2_set_power_mode(on ? BMA2x2_MODE_NORMAL : BMA2x2_MODE_DEEP_SUSPEND);
+	}
+
+	float3 get_acceleration() override {
+		u8 range = 0;
+		if(0 != bma2x2_get_range(&range)){
+			return float3(0,0,0);
+		}
+
+		//14 bit resolution. Hardcoded for now.
+		constexpr float max_resolution_val = (float)0x1fff;
+
+		float range_mul = 2.0f;
+		if(range == BMA2x2_RANGE_4G){
+			range_mul = 4.0f;
+		}else if(range == BMA2x2_RANGE_8G){
+			range_mul = 8.0f;
+		}else if(range == BMA2x2_RANGE_16G){
+			range_mul = 16.0f;
+		}
+
+		//BMA2x2_RANGE_2G
+		bma2x2_accel_data sample_xyz;
+
+
+		if(0 == bma2x2_read_accel_xyz(&sample_xyz)){
+			return float3(
+					(float)(sample_xyz.x) / max_resolution_val,
+					(float)(sample_xyz.y) / max_resolution_val,
+					(float)(sample_xyz.z) / max_resolution_val) * range_mul;
+		}else{
+			return float3(0,0,0);
+		}
+	}
+private:
+	static void SPI_begin(){
+		HAL_GPIO_WritePin(SS_ACC_GPIO_Port, SS_ACC_Pin, GPIO_PIN_RESET);
+	}
+
+	static void SPI_end(){
+		HAL_GPIO_WritePin(SS_ACC_GPIO_Port, SS_ACC_Pin, GPIO_PIN_SET);
+	}
+
+	static s8 SPI_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt){
+		u8 array[SPI_BUFFER_LEN]={0xFF};
+		u8 stringpos;
+
+
+		array[0] = reg_addr|0x80;
+		SPI_begin();
+		auto spi_result = HAL_SPI_TransmitReceive(spi, array, array, cnt+1, HAL_MAX_DELAY);
+		SPI_end();
+		for (stringpos = 0; stringpos < cnt; stringpos++) {
+			*(reg_data + stringpos) = array[stringpos+1];
+		}
+		return spi_result == HAL_OK ? 0 : -1;
+	}
+
+	static s8 SPI_bus_write(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt){
+		u8 array[SPI_BUFFER_LEN * 2];
+		u8 stringpos = 0;
+		for (stringpos = 0; stringpos < cnt; stringpos++) {
+
+			array[stringpos * 2] = (reg_addr++) & 0x7F;
+			array[stringpos * 2 + 1] = *(reg_data + stringpos);
+		}
+
+		SPI_begin();
+		auto spi_result = HAL_SPI_Transmit(spi, array, cnt * 2, HAL_MAX_DELAY);
+		SPI_end();
+
+		return spi_result == HAL_OK ? 0 : -1;
+	}
+
+	static void delay_msek(u32 msek){
+		HAL_Delay(msek);
+	}
+
+	static s8 BMA_bind_SPI(bma2x2_t* bma) {
+		bma->bus_write = SPI_bus_write;
+		bma->bus_read = SPI_bus_read;
+		bma->delay_msec = delay_msek;
+		return 0;
+	}
+
+	bma2x2_t _bma2x2;
+};
+
+
+#endif /* SRC_BMA_ACCELEROMETER_H_ */

firmware/copter_vldiscovery_board/inc/board.h

@@ -0,0 +1,16 @@
+/*
+ * board.h
+ *
+ *  Created on: 04 ÿíâ. 2016 ã.
+ *      Author: Sergey
+ */
+
+#ifndef BOARD_H_
+#define BOARD_H_
+
+#define CONSOLE_UART huart3
+
+#define RADIO_SPI hspi1
+
+
+#endif /* BOARD_H_ */

firmware/copter_vldiscovery_board/inc/sound.h

@@ -0,0 +1,116 @@
+/*
+ * sound.h
+ *
+ *  Created on: 03 ÿíâ. 2016 ã.
+ *      Author: Sergey
+ */
+
+#ifndef SOUND_H_
+#define SOUND_H_
+
+#include <cstdint>
+#include <cstddef>
+
+enum class SoundNodeType {
+	Wave,
+	Silence
+};
+
+struct SoundNode {
+	SoundNodeType type;
+	uint32_t freq;
+	uint32_t length; //in freq periods
+};
+
+struct Notes {
+	//First octave x100 mul
+	static constexpr uint32_t values[] = {
+			26163, 27718, 29366, 31113,
+			32963, 34923, 36999, 39200,
+			41530, 44000, 46616, 49388};
+	static constexpr uint32_t C() {return values[0]; }
+	static constexpr uint32_t C_d() {return values[1]; }
+	static constexpr uint32_t D() {return values[2]; }
+	static constexpr uint32_t D_d() {return values[3]; }
+	static constexpr uint32_t E() {return values[4]; }
+	static constexpr uint32_t F() {return values[5]; }
+	static constexpr uint32_t F_d() {return values[6]; }
+	static constexpr uint32_t G() {return values[7]; }
+	static constexpr uint32_t G_d() {return values[8]; }
+	static constexpr uint32_t A() {return values[9]; }
+	static constexpr uint32_t B_b() {return values[10]; }
+	static constexpr uint32_t B() {return values[11]; }
+};
+
+
+
+//int16_t get_sine(uint8_t phase8);
+uint8_t get_sine(uint8_t phase8);
+
+template<typename Base>
+class SoundPlayer : public Base {
+public:
+	uint8_t out;
+	bool is_playing;
+	SoundPlayer():out(0), is_playing(false), _loop(false), _nodes(nullptr), _nodes_count(0), _active_node(0), _sample_counter(0) {
+
+	}
+
+	void stop(){
+		if(is_playing){
+			Base::on_sound_stop();
+		}
+	}
+	template<size_t Count>
+	void play_sound(SoundNode(&nodes)[Count], bool loop = false){
+		play_sound(nodes, Count, loop);
+	}
+
+	void play_sound(SoundNode* nodes, size_t nodes_count, bool loop = false){
+		stop();
+		_loop = loop;
+		out = 0;
+		is_playing = true;
+		_nodes = nodes;
+		_nodes_count = nodes_count;
+		_active_node = 0;
+		_sample_counter = 0;
+		Base::on_sound_play();
+	}
+	void update(){
+		if(is_playing){
+			auto tmp = ((_sample_counter * 0x100 * (uint64_t)_nodes[_active_node].freq) / Base::get_sampling_rate()) / 100;
+			if(_nodes[_active_node].type == SoundNodeType::Silence){
+				out = 0;
+			} else {
+				uint8_t sample_interp = (uint8_t)(tmp % 0x100);
+				//out = sample_interp  < 128 ? 0 : 0xff;
+				out = (int32_t)get_sine(sample_interp);
+			}
+
+			_sample_counter++;
+			if (_sample_counter >= _nodes[_active_node].length) {
+				_active_node++;
+				if (_active_node >= _nodes_count) {
+					if(_loop){
+						_active_node = 0;
+					}else{
+						is_playing = false;
+						Base::on_sound_stop();
+					}
+				}
+				_sample_counter = 0;
+			}
+		}
+	}
+private:
+	bool _loop;
+	SoundNode* _nodes;
+	size_t _nodes_count;
+	size_t _active_node;
+	uint32_t _sample_counter;
+};
+
+
+
+#endif /* SOUND_H_ */

firmware/copter_vldiscovery_board/inc/sound_module.h

@@ -0,0 +1,41 @@
+/*
+ * sound_module.h
+ *
+ *  Created on: 03 ÿíâ. 2016 ã.
+ *      Author: Sergey
+ */
+
+#ifndef SOUND_MODULE_H_
+#define SOUND_MODULE_H_
+
+#include "stm32f1xx_hal.h"
+#include "sound.h"
+
+/*
+#define SOUND_SAMPLING_RATE 24000
+#define SOUND_MS_TO_SAMPLES(__ms) ((SOUND_SAMPLING_RATE * __ms) / 1000)
+
+extern DAC_HandleTypeDef hdac;
+extern TIM_HandleTypeDef htim6;
+
+class DacSoundPlayerBase {
+public:
+	static constexpr uint32_t get_sampling_rate() {
+		return SOUND_SAMPLING_RATE;
+	}
+	static void on_sound_play() {
+		HAL_DAC_Start(&hdac, DAC_CHANNEL_1);
+		HAL_TIM_Base_Start_IT(&htim6);
+	}
+	static void on_sound_stop() {
+		HAL_TIM_Base_Stop_IT(&htim6);
+		HAL_DAC_Stop(&hdac, DAC_CHANNEL_1);
+	}
+};
+
+
+typedef SoundPlayer<DacSoundPlayerBase> dac_sound_player;
+
+*/
+
+#endif /* SOUND_MODULE_H_ */

firmware/copter_vldiscovery_board/inc/uart_commands.h

@@ -0,0 +1,24 @@
+/*
+ * uart_commands.h
+ *
+ *  Created on: 02 ÿíâ. 2016 ã.
+ *      Author: Sergey
+ */
+
+#ifndef UART_COMMANDS_H_
+#define UART_COMMANDS_H_
+
+#include <cstdint>
+#include "uart_protocol.h"
+
+struct uart_commands {
+	static constexpr uint32_t led_write = FOURCC('L', 'e', 'd', 'W');
+	static constexpr uint32_t led_read = FOURCC('L', 'e', 'd', 'R');
+	static constexpr uint32_t motor_write = FOURCC('M', 't', 'r', 'W');
+	static constexpr uint32_t play_sound = FOURCC('P', 'S', 'n', 'd');
+	static constexpr uint32_t acc_read = FOURCC('A', 'c', 'c', 'R');
+	static constexpr uint32_t gyro_read = FOURCC('G', 'r', 'o', 'R');
+};
+
+
+#endif /* UART_COMMANDS_H_ */

firmware/copter_vldiscovery_board/inc/uart_protocol.h

@@ -0,0 +1,55 @@
+/*
+ * uart_protocol.h
+ *
+ *  Created on: 02 ÿíâ. 2016 ã.
+ *      Author: Sergey
+ */
+
+#ifndef UART_PROTOCOL_H_
+#define UART_PROTOCOL_H_
+
+#include <cstdint>
+
+#pragma pack(push, 1)
+struct UartCommandHeader {
+	uint32_t id;
+	uint8_t payloadSize;
+};
+
+struct UartCommand {
+	UartCommandHeader header;
+	static constexpr int MaxPayloadSize = 32;
+	uint8_t payload[MaxPayloadSize];
+};
+
+#define FOURCC(a,b,c,d) ( (uint32_t) ((((uint32_t)d)<<24) | (((uint32_t)c)<<16) | (((uint32_t)b)<<8) | ((uint32_t)a)) )
+
+#pragma pack(pop)
+
+struct UartCommandReader{
+	uint32_t offset;
+	UartCommand* cmd;
+	UartCommandReader(UartCommand* _cmd):offset(0), cmd(_cmd){
+	}
+
+	template<typename T>
+	T read(){
+		return *((T*)&cmd->payload[offset++]);
+	}
+};
+
+struct UartCommandWriter{
+	UartCommand* cmd;
+	UartCommandWriter(UartCommand* _cmd) : cmd(_cmd){
+		cmd->header.payloadSize = 0;
+	}
+
+	template<typename T>
+	UartCommandWriter& write(const T& val){
+		((T*)&cmd->payload[cmd->header.payloadSize])[0] = val;
+		cmd->header.payloadSize += sizeof(T);
+		return *this;
+	}
+};
+
+#endif /* UART_PROTOCOL_H_ */