Merge branch 'sync-with-controle-remote' into 'master'

[sync] merge code of Control Remote

See merge request !3

Algorithm/pid.c

@@ -30,13 +30,15 @@ void pid_init(pid_struct_t *pid,
               float ki,
               float kd,
               float i_max,
-              float out_max)
+              float out_max,
+							int 	mode)
 {
   pid->kp      = kp;
   pid->ki      = ki;
   pid->kd      = kd;
   pid->i_max   = i_max;
   pid->out_max = out_max;
+	pid->mode		 = mode;
 }
 
 /**
@@ -52,9 +54,12 @@ float pid_calc(pid_struct_t *pid, float ref, float fdb)
   pid->fdb = fdb;
   pid->err[1] = pid->err[0];
   pid->err[0] = pid->ref - pid->fdb;
-	if(pid->err[0]>4096){pid->err[0] = pid->err[0] - 8192;}
-	if(pid->err[0]<-4096){pid->err[0] = pid->err[0] + 8192;}
-  
+	if (pid->mode == 0)
+	{
+		if(pid->err[0]>4096){pid->err[0] = pid->err[0] - 8192;}
+		if(pid->err[0]<-4096){pid->err[0] = pid->err[0] + 8192;}
+  }
+		
   pid->p_out  = pid->kp * pid->err[0];
   pid->i_out += pid->ki * pid->err[0];
   pid->d_out  = pid->kd * pid->err[0] - pid->err[1];

Algorithm/pid.h

@@ -22,6 +22,7 @@
 
 typedef struct _pid_struct_t
 {
+	int mode; 			// mode = 0 : position, 1 : speed
   float kp;
   float ki;
   float kd;
@@ -43,7 +44,8 @@ void pid_init(pid_struct_t *pid,
               float ki,
               float kd,
               float i_max,
-              float out_max);
+              float out_max,
+							int 	mode);
               
 float  pid_calc(pid_struct_t *pid, float ref, float fdb);
 

Device/Remote_Control.c

+/**
+  ******************************************************************************
+  * @file    Remote_Control.c
+  * @author  DJI 
+  * @version V1.0.0
+  * @date    2015/11/15
+  * @brief   
+  ******************************************************************************
+  * @attention
+  *
+  ******************************************************************************
+  */
+#include "Remote_Control.h"
+#define LED2_Pin GPIO_PIN_7
+#define LED2_GPIO_Port GPIOE
+RC_Type remote_control;
+uint32_t  Latest_Remote_Control_Pack_Time = 0;
+uint32_t  LED_Flash_Timer_remote_control = 0;
+/*******************************************************************************************
+  * @Func		void Callback_RC_Handle(RC_Type* rc, uint8_t* buff)
+  * @Brief  DR16接收机协议解码程序
+  * @Param		RC_Type* rc 存储遥控器数据的结构体  uint8_t* buff 用于解码的缓存
+  * @Retval		None
+  * @Date    
+ *******************************************************************************************/
+void Callback_RC_Handle(RC_Type* rc, uint8_t* buff)
+{
+//	rc->ch1 = (*buff | *(buff+1) << 8) & 0x07FF;	offset  = 1024
+	rc->ch1 = (buff[0] | buff[1]<<8) & 0x07FF;
+	rc->ch1 -= 1024;
+	rc->ch2 = (buff[1]>>3 | buff[2]<<5 ) & 0x07FF;
+	rc->ch2 -= 1024;
+	rc->ch3 = (buff[2]>>6 | buff[3]<<2 | buff[4]<<10) & 0x07FF;
+	rc->ch3 -= 1024;
+	rc->ch4 = (buff[4]>>1 | buff[5]<<7) & 0x07FF;		
+	rc->ch4 -= 1024;
+	
+	rc->switch_left = ( (buff[5] >> 4)& 0x000C ) >> 2;
+	rc->switch_right =  (buff[5] >> 4)& 0x0003 ;
+	
+	rc->mouse.x = buff[6] | (buff[7] << 8);	//x axis
+	rc->mouse.y = buff[8] | (buff[9] << 8);
+	rc->mouse.z = buff[10]| (buff[11] << 8);
+	
+	rc->mouse.press_left 	= buff[12];	// is pressed?
+	rc->mouse.press_right = buff[13];
+	
+	rc->keyBoard.key_code = buff[14] | buff[15] << 8; //key borad code
+	
+	Latest_Remote_Control_Pack_Time = HAL_GetTick();
+	
+	if(Latest_Remote_Control_Pack_Time - LED_Flash_Timer_remote_control>500){
+			
+			HAL_GPIO_TogglePin(LED2_GPIO_Port,LED2_Pin);
+			
+			LED_Flash_Timer_remote_control = Latest_Remote_Control_Pack_Time;
+		
+			
+	}
+	
+}
+
+extern uint16_t TIM_COUNT[];
+int16_t HighTime;
+
+
+/*******************************************************************************************
+  * @Func		void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
+  * @Brief  PWM接收机脉宽计算
+  * @Param		TIM_HandleTypeDef *htim 用于测量PWM脉宽的定时器。
+  * @Retval		None
+  * @Date    
+ *******************************************************************************************/
+void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
+{
+
+	HighTime = (TIM_COUNT[1] - TIM_COUNT[0])>0?(TIM_COUNT[1] - TIM_COUNT[0]):((TIM_COUNT[1] - TIM_COUNT[0])+10000);
+	
+	Latest_Remote_Control_Pack_Time = HAL_GetTick();
+			
+  remote_control.ch4 = (HighTime - 4000)*660/4000;
+	
+	if(Latest_Remote_Control_Pack_Time - LED_Flash_Timer_remote_control>500){
+			
+			HAL_GPIO_TogglePin(LED2_GPIO_Port,LED2_Pin);		
+			LED_Flash_Timer_remote_control = Latest_Remote_Control_Pack_Time;
+					
+	}
+	
+}

Device/Remote_Control.h

+/**
+  ******************************************************************************
+  * @file    Remote_Control.h
+  * @author  Ginger
+  * @version V1.0.0
+  * @date    2015/11/14
+  * @brief   
+  ******************************************************************************
+  * @attention
+  *
+  ******************************************************************************
+  */
+	
+#ifndef __RC__
+#define __RC__
+/* Includes ------------------------------------------------------------------*/
+#include "stm32f4xx_hal.h"
+
+#define RC_Frame_Lentgh		18
+
+typedef struct {
+	int16_t ch1;	//each ch value from -364 -- +364
+	int16_t ch2;
+	int16_t ch3;
+	int16_t ch4;
+	
+	uint8_t switch_left;	//3 value
+	uint8_t switch_right;
+	
+	struct {
+		int16_t x;
+		int16_t y;
+		int16_t z;
+	
+		uint8_t press_left;
+		uint8_t press_right;
+	}mouse;
+	
+	struct {
+		uint16_t key_code;
+/**********************************************************************************
+   * ͨ:15   14   13   12   11   10   9   8   7   6     5     4   3   2   1
+   *          V    C    X	  Z    G    F    R   E   Q  CTRL  SHIFT  D   A   S   W
+************************************************************************************/
+
+	}keyBoard;
+	
+
+}RC_Type;
+
+
+
+enum{
+	Switch_Up = 1,
+	Switch_Middle = 3,
+	Switch_Down = 2,
+};
+
+enum{
+	Key_W,
+	Key_S,
+	//...
+};
+
+extern RC_Type remote_control;
+extern uint32_t  Latest_Remote_Control_Pack_Time ;
+void Callback_RC_Handle(RC_Type* rc, uint8_t* buff);
+#endif
+
+

Device/list_motor.c

@@ -3,18 +3,23 @@
 #include "list_motor.h"
 
 motor_t gm6020;
-const pid_struct_t M3508_pid_speed = {
-		2, 0.005, 2, 16000, 16000
+motor_t m3508_frontleft;
+motor_t m3508_frontright;
+motor_t m3508_backleft;
+motor_t m3508_backright;
+
+const pid_struct_t M3508_pid_speed = { //A VERIFIER
+		1, 2, 0.005, 2, 16000, 16000
 	};
-const pid_struct_t M3508_pid_pos = {
-		0.1, 0.005, 0, 16000, 16000
+const pid_struct_t M3508_pid_pos = { //A VERIFIER
+		0, 0.1, 0.005, 0, 16000, 16000
 	};
 
-const pid_struct_t GM6020_pid_speed = {
-		40, 3, 0, 30000, 30000
+const pid_struct_t GM6020_pid_speed = { //A VERIFIER
+		1, 40, 10, 0, 30000, 30000
 	};
-const pid_struct_t GM6020_pid_pos = {
-		30, 4, 15, 10000, 10000
+const pid_struct_t GM6020_pid_pos = { //A VERIFIER
+		0, 12, 3, 5, 10000, 10000
 	};
 
 void motors_init(void){
@@ -24,5 +29,33 @@ void motors_init(void){
 	gm6020.can_info.rx_id = 0x205; 
 	gm6020.pid_pos = GM6020_pid_pos;
 	gm6020.pid_speed = GM6020_pid_speed;
+	
+	m3508_frontleft.type = M3508;
+	m3508_frontleft.can_info.can_id = 0;
+	m3508_frontleft.can_info.tx_id = 0;
+	m3508_frontleft.can_info.rx_id = 0x201; 
+	m3508_frontleft.pid_pos = M3508_pid_pos;
+	m3508_frontleft.pid_speed = M3508_pid_speed;
+	
+	m3508_frontright.type = M3508;
+	m3508_frontright.can_info.can_id = 0;
+	m3508_frontright.can_info.tx_id = 0;
+	m3508_frontright.can_info.rx_id = 0x202; 
+	m3508_frontright.pid_pos = M3508_pid_pos;
+	m3508_frontright.pid_speed = M3508_pid_speed;
+	
+	m3508_backleft.type = M3508;
+	m3508_backleft.can_info.can_id = 0;
+	m3508_backleft.can_info.tx_id = 0;
+	m3508_backleft.can_info.rx_id = 0x203; 
+	m3508_backleft.pid_pos = M3508_pid_pos;
+	m3508_backleft.pid_speed = M3508_pid_speed;
+	
+	m3508_backright.type = M3508;
+	m3508_backright.can_info.can_id = 0;
+	m3508_backright.can_info.tx_id = 0;
+	m3508_backright.can_info.rx_id = 0x204; 
+	m3508_backright.pid_pos = M3508_pid_pos;
+	m3508_backright.pid_speed = M3508_pid_speed;
 }
 

Device/list_motor.h

@@ -9,7 +9,9 @@ extern motor_t m3508_backright;
 extern motor_t gm6020_down;
 extern motor_t gm6020_up;
 extern motor_t m2006;
+
 extern motor_t gm6020;
+extern motor_t m3508;
 
 extern const pid_struct_t M3508_pid_speed;
 extern const pid_struct_t M3508_pid_pos;

Device/motor.c

 #include "motor.h"
 #include "oled.h"
 #include "pid.h"
-//#include "list_motor.h"
 
 void set_motor_voltage(uint8_t id, motor_t *motor1, motor_t *motor2, motor_t *motor3, motor_t *motor4) {
     int16_t sp1, sp2, sp3, sp4;
@@ -23,42 +22,37 @@ static int16_t correct_output(motor_t *motor) {
 }
 
 
-static void get_6020_data(motor_t* motor, uint8_t buf[CAN_DATA_SIZE]){
-	motor->info.GM6020_info.angle = (int16_t)(buf[0] << 8 | buf[1]);
-	motor->info.GM6020_info.speed = (int16_t)(buf[2] << 8 | buf[3]);
-	motor->info.GM6020_info.current = (int16_t)(buf[4] << 8 | buf[5]);
-	motor->info.GM6020_info.temp = (int8_t)(buf[6]);
-	/*
-	oled_clear(Pen_Clear);
-	oled_printf(1,1,"Val: %i", motor->info.GM6020_info.angle);
-	*/
+static void get_6020_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
+	motor->info.GM6020_info.angle = (int16_t)(motor_buf[0] << 8 | motor_buf[1]);
+	motor->info.GM6020_info.speed = (int16_t)(motor_buf[2] << 8 | motor_buf[3]);
+	motor->info.GM6020_info.current = (int16_t)(motor_buf[4] << 8 | motor_buf[5]);
+	motor->info.GM6020_info.temp = (int8_t)(motor_buf[6]);
 }
 
-static void get_3508_data(motor_t* motor, uint8_t buf[CAN_DATA_SIZE]){
-	motor->info.M3508_info.angle = (int16_t)(buf[0] << 8 | buf[1]);
-	motor->info.M3508_info.speed = (int16_t)(buf[2] << 8 | buf[3]);
-	motor->info.M3508_info.current = (int16_t)(buf[4] << 8 | buf[5]);
-	motor->info.M3508_info.temp = (int8_t)(buf[6]);
+static void get_3508_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
+	motor->info.M3508_info.angle = (int16_t)(motor_buf[0] << 8 | motor_buf[1]);
+	motor->info.M3508_info.speed = (int16_t)(motor_buf[2] << 8 | motor_buf[3]);
+	motor->info.M3508_info.current = (int16_t)(motor_buf[4] << 8 | motor_buf[5]);
+	motor->info.M3508_info.temp = (int8_t)(motor_buf[6]);
 }
 
 void run_motor_from_command(motor_t *motor, pid_struct_t pid_param, float target, float fdb)
 {
-	
 	motor->voltage = pid_calc(&pid_param, target, fdb);
 }
 
 
 uint8_t get_motor_data(motor_t *motor) {
-    uint8_t buf[CAN_DATA_SIZE];
-    can1_read(motor->can_info.rx_id, buf);
+    static uint8_t motor_buf[CAN_DATA_SIZE];
+    can1_read(motor->can_info.rx_id, motor_buf);
 		
 	
     switch (motor->type) {
 			case GM6020:
-				get_6020_data(motor, buf);
+				get_6020_data(motor, motor_buf);
 				return 1;
 			case M3508:
-				get_3508_data(motor, buf);
+				get_3508_data(motor, motor_buf);
 				return 1;
 			default:
 				return 0;

Driver/bsp_can.h

@@ -22,7 +22,7 @@
 #include <stdio.h>
 #include <string.h>
 
-#define FEEDBACK_ID_BASE      0x204
+#define FEEDBACK_ID_BASE      0x201
 #define CAN_CONTROL_ID_BASE   0x200
 #define CAN_CONTROL_ID_EXTEND 0x1ff
 #define MOTOR_MAX_NUM         7

Driver/bsp_pwm.c

@@ -68,18 +68,14 @@ void PWM_SetAllDuty(TIM_HandleTypeDef *tim, float duty_ch1, float duty_ch2, floa
   * @retval None
   */
 void PWM_ScaleAll(TIM_HandleTypeDef *tim){ //il faudrait jouer sur l'allumage des ports d'alimentation des snails (voir Nathan pour plus de dtails)
-	tim->Instance->CCR1 = (PWM_RESOLUTION*0.2) - 1;
-	tim->Instance->CCR2 = (PWM_RESOLUTION*0.2) - 1;
-	tim->Instance->CCR3 = (PWM_RESOLUTION*0.2) - 1;
-	tim->Instance->CCR4 = (PWM_RESOLUTION*0.2) - 1;
+	HAL_GPIO_WritePin(GPIOH, POWER1_CTRL_Pin, GPIO_PIN_RESET); // switch off 24v power
+	PWM_SetAllDuty(&htim1,1,1,1,1);
+	HAL_Delay(10);
 	
+	HAL_GPIO_WritePin(GPIOH, POWER1_CTRL_Pin, GPIO_PIN_SET); // switch on 24v power
 	HAL_Delay(3500);
 	
-	tim->Instance->CCR1 = (PWM_RESOLUTION*0.1) - 1;
-	tim->Instance->CCR2 = (PWM_RESOLUTION*0.1) - 1;
-	tim->Instance->CCR3 = (PWM_RESOLUTION*0.1) - 1;
-	tim->Instance->CCR4 = (PWM_RESOLUTION*0.1) - 1;
-	
+	PWM_SetAllDuty(&htim1,0,0,0,0);
 	HAL_Delay(500);
 	
 }

Driver/bsp_uart.c

+/**
+ ***************************************(C) COPYRIGHT 2018 DJI***************************************
+ * @file       bsp_uart.c
+ * @brief      this file contains rc data receive and processing function
+ * @note       
+ * @Version    V1.0.0
+ * @Date       Jan-30-2018
+ ***************************************(C) COPYRIGHT 2018 DJI***************************************
+ */
+                                                                                                              
+#include "string.h"
+#include "stdlib.h"
+#include "bsp_uart.h"
+#include "usart.h"
+#include "main.h"
+#include "oled.h"
+uint8_t   dbus_buf[DBUS_BUFLEN];
+rc_info_t rc;
+
+
+/**
+  * @brief      enable global uart it and do not use DMA transfer done it
+  * @param[in]  huart: uart IRQHandler id
+  * @param[in]  pData: receive buff 
+  * @param[in]  Size:  buff size
+  * @retval     set success or fail
+  */
+static int uart_receive_dma_no_it(UART_HandleTypeDef* huart, uint8_t* pData, uint32_t Size)
+{
+  uint32_t tmp1 = 0;
+
+  tmp1 = huart->RxState;
+	
+	if (tmp1 == HAL_UART_STATE_READY)
+	{
+		if ((pData == NULL) || (Size == 0))
+		{
+			return HAL_ERROR;
+		}
+
+		huart->pRxBuffPtr = pData;
+		huart->RxXferSize = Size;
+		huart->ErrorCode  = HAL_UART_ERROR_NONE;
+
+		/* Enable the DMA Stream */
+		HAL_DMA_Start(huart->hdmarx, (uint32_t)&huart->Instance->DR, (uint32_t)pData, Size);
+
+		/* 
+		 * Enable the DMA transfer for the receiver request by setting the DMAR bit
+		 * in the UART CR3 register 
+		 */
+		SET_BIT(huart->Instance->CR3, USART_CR3_DMAR);
+
+		return HAL_OK;
+	}
+	else
+	{
+		return HAL_BUSY;
+	}
+}
+
+/**
+  * @brief      returns the number of remaining data units in the current DMAy Streamx transfer.
+  * @param[in]  dma_stream: where y can be 1 or 2 to select the DMA and x can be 0
+  *             to 7 to select the DMA Stream.
+  * @retval     The number of remaining data units in the current DMAy Streamx transfer.
+  */
+uint16_t dma_current_data_counter(DMA_Stream_TypeDef *dma_stream)
+{
+  /* Return the number of remaining data units for DMAy Streamx */
+  return ((uint16_t)(dma_stream->NDTR));
+}
+
+
+
+/**
+  * @brief       handle received rc data
+  * @param[out]  rc:   structure to save handled rc data
+  * @param[in]   buff: the buff which saved raw rc data
+  * @retval 
+  */
+void rc_callback_handler(rc_info_t *rc, uint8_t *buff)
+{
+	
+  rc->ch4 = (buff[0] | buff[1] << 8) & 0x07FF; //0x07FF met la commande sur 11 bits (donc 2048 max)
+  rc->ch4 -= 1024; //on centre sur 0
+  rc->ch3 = (buff[1] >> 3 | buff[2] << 5) & 0x07FF;
+  rc->ch3 -= 1024;
+  rc->ch2 = (buff[2] >> 6 | buff[3] << 2 | buff[4] << 10) & 0x07FF;
+  rc->ch2 -= 1024;
+  rc->ch1 = (buff[4] >> 1 | buff[5] << 7) & 0x07FF;
+  rc->ch1 -= 1024;
+
+  rc->sw1 = ((buff[5] >> 4) & 0x000C) >> 2;
+  rc->sw2 = (buff[5] >> 4) & 0x0003;
+  
+  if ((abs(rc->ch1) > 660) || \
+      (abs(rc->ch2) > 660) || \
+      (abs(rc->ch3) > 660) || \
+      (abs(rc->ch4) > 660))
+  {
+    memset(rc, 0, sizeof(rc_info_t));
+  }		
+}
+
+/**
+  * @brief      clear idle it flag after uart receive a frame data
+  * @param[in]  huart: uart IRQHandler id
+  * @retval  
+  */
+static void uart_rx_idle_callback(UART_HandleTypeDef* huart)
+{
+	/* clear idle it flag avoid idle interrupt all the time */
+	__HAL_UART_CLEAR_IDLEFLAG(huart);
+	
+
+	/* handle received data in idle interrupt */
+	if (huart == &DBUS_HUART)
+	{
+		/* clear DMA transfer complete flag */
+		__HAL_DMA_DISABLE(huart->hdmarx);
+
+		/* handle dbus data dbus_buf from DMA */
+		if ((DBUS_MAX_LEN - dma_current_data_counter(huart->hdmarx->Instance)) == DBUS_BUFLEN)
+		{
+			rc_callback_handler(&rc, dbus_buf);	
+		}
+		
+		/* restart dma transmission */
+		__HAL_DMA_SET_COUNTER(huart->hdmarx, DBUS_MAX_LEN);
+		__HAL_DMA_ENABLE(huart->hdmarx);
+	}
+}
+
+/**
+  * @brief      callback this function when uart interrupt 
+  * @param[in]  huart: uart IRQHandler id
+  * @retval  
+  */
+void uart_receive_handler(UART_HandleTypeDef *huart)
+{
+	if (__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE) && 
+			__HAL_UART_GET_IT_SOURCE(huart, UART_IT_IDLE))
+	{
+		uart_rx_idle_callback(huart);
+	}
+	
+}
+
+/**
+  * @brief   initialize dbus uart device 
+  * @param   
+  * @retval  
+  */
+void dbus_uart_init(void)
+{
+	/* open uart idle it */
+	__HAL_UART_CLEAR_IDLEFLAG(&DBUS_HUART);
+	__HAL_UART_ENABLE_IT(&DBUS_HUART, UART_IT_IDLE);
+
+	uart_receive_dma_no_it(&DBUS_HUART, dbus_buf, DBUS_MAX_LEN);
+}
+
+
+

Driver/bsp_uart.h

+/**
+ ***************************************(C) COPYRIGHT 2018 DJI***************************************
+ * @file       bsp_uart.h
+ * @brief      this file contains the common defines and functions prototypes for 
+ *             the bsp_uart.c driver
+ * @note         
+ * @Version    V1.0.0
+ * @Date       Jan-30-2018      
+ ***************************************(C) COPYRIGHT 2018 DJI***************************************
+ */
+
+#ifndef __BSP_UART_H__
+#define __BSP_UART_H__
+
+#include "usart.h"
+
+#define UART_RX_DMA_SIZE (1024)
+#define DBUS_MAX_LEN     (50)
+#define DBUS_BUFLEN      (18)
+#define DBUS_HUART       huart1 /* for dji remote controler reciever */
+/** 
+  * @brief  remote control information
+  */
+typedef __packed struct
+{
+  /* rocker channel information */
+  int16_t ch1;
+  int16_t ch2;
+  int16_t ch3;
+  int16_t ch4;
+
+  /* left and right lever information */
+  uint8_t sw1;
+  uint8_t sw2;
+} rc_info_t;
+
+void uart_receive_handler(UART_HandleTypeDef *huart);
+void dbus_uart_init(void);
+void USART_SendData(USART_TypeDef* USARTx, uint16_t Data);
+#endif
+