HFUT_Firmament_EC
/
sentry_gimbal_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update some motor defs

This commit is contained in:
NeoZng 2022-10-30 22:19:13 +08:00
parent 5f59ebb916
commit 6441982964
11 changed files with 224 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.vscode/launch.json vendored
View File

@ -2,7 +2,7 @@
"version": "0.2.0",
"configurations": [
{
"name": "Cortex Debug",
"name": "Debug-dap",
"cwd": "${workspaceRoot}",
"executable": "${workspaceRoot}\\build\\basic_framework.elf",
"request": "launch",
@ -17,6 +17,20 @@
"armToolchainPath": "D:\\gcc-arm-none-eabi\\bin",
// path to your gcc-arm-none-eabi/arm-none-eabi-gdb.exe
"gdbPath": "D:\\gcc-arm-none-eabi\\bin\\arm-none-eabi-gdb.exe"
},
{
"name": "Debug-jlink",
"cwd": "${workspaceFolder}",
"executable": "${workspaceRoot}\\build\\basic_framework.elf",
"request": "launch",
"type": "cortex-debug",
"device": "STM32F407IG",
"runToEntryPoint": "main",
"showDevDebugOutput": "none",
"servertype": "jlink",
"interface": "swd",
"armToolchainPath": "D:\\gcc-arm-none-eabi\\bin",
"gdbPath": "D:\\gcc-arm-none-eabi\\bin\\arm-none-eabi-gdb.exe"
}
]
}

2
Src/main.c
View File

@ -37,6 +37,7 @@
#include "bsp_can.h"
#include "can.h"
#include "LK9025.h"
#include "dji_motor.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
@ -114,7 +115,6 @@ int main(void)
MX_USART1_UART_Init();
MX_USART6_UART_Init();
/* USER CODE BEGIN 2 */
/* USER CODE END 2 */
/* Call init function for freertos objects (in freertos.c) */

41
bsp/bsp_can.c
View File

@ -3,6 +3,7 @@
#include <stdlib.h>
#include "memory.h"
/* can instance ptrs storage, used for recv callback */
static can_instance* instance[MX_REGISTER_DEVICE_CNT];
/**
@ -52,7 +53,11 @@ static void CANServiceInit()
HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
}
can_instance* CANRegister(uint8_t tx_id,uint8_t rx_id,CAN_HandleTypeDef* can_handle,can_callback module_callback)
/* -----------------------two callable function -----------------------*/
can_instance* CANRegister(can_instance_config config)
{
static uint8_t idx;
if(!idx)
@ -60,25 +65,33 @@ can_instance* CANRegister(uint8_t tx_id,uint8_t rx_id,CAN_HandleTypeDef* can_han
CANServiceInit();
}
instance[idx]=(can_instance*)malloc(sizeof(can_instance));
instance[idx]->can_handle=can_handle;
instance[idx]->tx_id=tx_id;
instance[idx]->rx_id=rx_id;
instance[idx]->can_module_callback=module_callback;
instance[idx]->txconf.StdId=config.tx_id;
instance[idx]->txconf.IDE=CAN_ID_STD;
instance[idx]->txconf.RTR=CAN_RTR_DATA;
instance[idx]->txconf.DLC=0x08;
instance[idx]->can_handle=config.can_handle;
instance[idx]->tx_id=config.tx_id;
instance[idx]->rx_id=config.rx_id;
instance[idx]->can_module_callback=config.can_module_callback;
CANAddFilter(instance[idx]);
return instance[idx++];
}
void CANTransmit(can_instance* _instance)
{
CAN_TxHeaderTypeDef txconf;
txconf.StdId=_instance->tx_id;
txconf.IDE=CAN_ID_STD;
txconf.RTR=CAN_RTR_DATA;
txconf.DLC=0x08;
while (HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0);
HAL_CAN_AddTxMessage(_instance->can_handle, &txconf, _instance->tx_buff, &_instance->tx_mailbox);
while(HAL_CAN_GetTxMailboxesFreeLevel(_instance->can_handle) == 0);
HAL_CAN_AddTxMessage(_instance->can_handle, &_instance->txconf, _instance->tx_buff, &_instance->tx_mailbox);
}
/* -----------------------belows are callback definitions--------------------------*/
/**
* @brief this func will recv data from @param:fifox to a tmp can_rx_buff
* then, all the instances will be polling to check which should recv this pack of data
@ -102,7 +115,8 @@ static void CANFIFOxCallback(CAN_HandleTypeDef* _hcan,uint32_t fifox)
}
}
/* ATTENTION: two CAN device in STM32 share two FIFO */
/* ATTENTION: two CAN devices in STM32 share two FIFOs */
/* functions below will call CANFIFOxCallback() to further process message from a specific CAN device */
/**
* @brief rx fifo callback. Once FIFO_0 is full,this func would be called
@ -114,6 +128,7 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
CANFIFOxCallback(hcan, CAN_RX_FIFO0);
}
/**
* @brief rx fifo callback. Once FIFO_1 is full,this func would be called
*

25
bsp/bsp_can.h
View File

@ -9,21 +9,35 @@
#define MX_CAN_FILTER_CNT (4 * 14) // temporarily useless
#define DEVICE_CAN_CNT 2 // CAN1,CAN2
/* can instance typedef, every module registered to CAN should have this variable */
typedef struct tmp
typedef struct _
{
CAN_HandleTypeDef* can_handle;
CAN_TxHeaderTypeDef txconf;
uint32_t tx_id;
uint32_t tx_mailbox;
uint8_t tx_buff[8];
uint8_t rx_buff[8];
uint32_t rx_id;
void (*can_module_callback)(struct tmp*); // callback needs an instance to tell among registered ones
void (*can_module_callback)(struct _*); // callback needs an instance to tell among registered ones
} can_instance;
/* this structure is used as initialization*/
typedef struct
{
CAN_HandleTypeDef* can_handle;
uint32_t tx_id;
uint32_t rx_id;
void (*can_module_callback)(can_instance*);
} can_instance_config;
/* module callback,which resolve protocol when new mesg arrives*/
typedef void (*can_callback)(can_instance*);
/**
* @brief transmit mesg through CAN device
*
@ -31,13 +45,14 @@ typedef void (*can_callback)(can_instance*);
*/
void CANTransmit(can_instance* _instance);
/**
* @brief Register a module to CAN service,remember to call this before using a CAN device
*
* @param _instance can instance owned by a specific device, remember to initialize it!
*
* @param config init config
* @return can_instance* can instance owned by module
*/
can_instance* CANRegister(uint8_t tx_id,uint8_t rx_id,CAN_HandleTypeDef* can_handle,can_callback module_callback);
can_instance* CANRegister(can_instance_config config);
#endif

4
modules/motor/HT04.c
View File

@ -36,11 +36,11 @@ static void DecodeJoint(can_instance* motor_instance)
}
}
joint_instance* HTMotorInit(CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id)
joint_instance* HTMotorInit(can_instance_config config)
{
static uint8_t idx;
joint_motor_info[idx]=(joint_instance*)malloc(sizeof(joint_instance));
joint_motor_info[idx++]->motor_can_instace=CANRegister(tx_id,rx_id,_hcan,DecodeJoint);
joint_motor_info[idx++]->motor_can_instace=CANRegister(config);
}
void JointControl(joint_instance* _instance,float current)

2
modules/motor/HT04.h
View File

@ -32,7 +32,7 @@ typedef enum
CMD_ZERO_POSITION = 0xfe
} joint_mode;
joint_instance* HTMotorInit(CAN_HandleTypeDef* _hcan,uint8_t tx_id,uint8_t rx_id);
joint_instance* HTMotorInit(can_instance_config config);
void JointControl(joint_instance* _instance,float current);

1
modules/motor/LK9025.h
View File

@ -11,6 +11,7 @@
#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
typedef struct //9025
{
uint16_t last_ecd;

87
modules/motor/dji_motor.c
View File

@ -1,7 +1,57 @@
#include "dji_motor.h"
static dji_motor_instance* dji_motor_info[DJI_MOTOR_CNT];
static dji_motor_instance* dji_motor_info[DJI_MOTOR_CNT]={NULL};
// can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
// can2: [0]:0x1FF,[1]:0x200,[2]:0x2FF
static can_instance sender_assignment[6]=
{
[0]={.can_handle=&hcan1,.txconf.StdId=0x1ff,.txconf.IDE=CAN_ID_STD,.txconf.RTR=CAN_RTR_DATA,.txconf.DLC=0x08,.tx_buff={0}},
[1]={.can_handle=&hcan1,.txconf.StdId=0x200,.txconf.IDE=CAN_ID_STD,.txconf.RTR=CAN_RTR_DATA,.txconf.DLC=0x08,.tx_buff={0}},
[2]={.can_handle=&hcan1,.txconf.StdId=0x2ff,.txconf.IDE=CAN_ID_STD,.txconf.RTR=CAN_RTR_DATA,.txconf.DLC=0x08,.tx_buff={0}},
[3]={.can_handle=&hcan2,.txconf.StdId=0x1ff,.txconf.IDE=CAN_ID_STD,.txconf.RTR=CAN_RTR_DATA,.txconf.DLC=0x08,.tx_buff={0}},
[4]={.can_handle=&hcan2,.txconf.StdId=0x200,.txconf.IDE=CAN_ID_STD,.txconf.RTR=CAN_RTR_DATA,.txconf.DLC=0x08,.tx_buff={0}},
[5]={.can_handle=&hcan2,.txconf.StdId=0x2ff,.txconf.IDE=CAN_ID_STD,.txconf.RTR=CAN_RTR_DATA,.txconf.DLC=0x08,.tx_buff={0}},
};
/**
* @brief
*
* @param idx
*/
static void MotorSenderGrouping(uint8_t idx,can_instance_config config)
{
uint8_t motor_id=config.tx_id;
uint8_t motor_rx_id;
uint8_t motor_send_num;
uint8_t motor_grouping;
switch (dji_motor_info[idx]->motor_type)
{
case M2006:
case M3508:
if(motor_id<5)
{
}
else
{
}
break;
case GM6020:
if(motor_id<5)
{
}
else
{
}
break;
}
}
static void DecodeDJIMotor(can_instance* _instance)
{
@ -19,17 +69,44 @@ static void DecodeDJIMotor(can_instance* _instance)
}
}
dji_motor_instance* DJIMotorInit(CAN_HandleTypeDef* _hcan,uint16_t tx_id,uint16_t rx_id)
dji_motor_instance* DJIMotorInit(can_instance_config config,
Motor_Controller_s controller_config,
Motor_Control_Setting_s motor_setting,
Motor_Controller_Init_s controller_init,
Motor_Type_e type)
{
static uint8_t idx;
static uint8_t idx; // register idx
dji_motor_info[idx]=(dji_motor_instance*)malloc(sizeof(dji_motor_instance));
dji_motor_info[idx++]->motor_can_instance=CANRegister(tx_id,rx_id,_hcan,DecodeDJIMotor);
// motor setting
dji_motor_info[idx]->motor_type=type;
dji_motor_info[idx]->motor_settings=motor_setting;
// motor controller init @todo : PID init
dji_motor_info[idx]->motor_settings.angle_feedback_source=motor_setting.angle_feedback_source;
dji_motor_info[idx]->motor_settings.speed_feedback_source=motor_setting.speed_feedback_source;
// group motors, because 4 motors share the same CAN control message
MotorSenderGrouping(idx,config);
// register motor to CAN bus
dji_motor_info[idx]->motor_can_instance=CANRegister(config);
return dji_motor_info[idx++];
}
void DJIMotorSetRef();
void DJIMotorSetRef()
{
}
void DJIMotorControl()
{
for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
{
}
}

37
modules/motor/dji_motor.h
View File

@ -7,8 +7,13 @@
#include "controller.h"
#include "motor_def.h"
/**
* @brief DJI intelligent motor typedef
*
*/
typedef struct
{
/* motor measurement recv from CAN feedback */
struct
{
uint16_t ecd;
@ -20,25 +25,29 @@ typedef struct
int32_t total_angle;
} motor_measure;
struct
{
closeloop_e close_loop_type;
reverse_flag_e reverse_flag;
feedback_source_e angle_feedback_source;
feedback_source_e speed_feedback_source;
} controll_state;
/* basic config of a motor*/
Motor_Control_Setting_s motor_settings;
float* other_angle_feedback_ptr;
float* other_speed_feedback_ptr;
PID_t* current_PID;
PID_t* speed_PID;
PID_t* angle_PID;
/* controller used in the motor (3 loops)*/
Motor_Controller_s motor_controller;
/* the CAN instance own by motor instance*/
can_instance *motor_can_instance;
/* sender assigment*/
uint8_t sender_group;
uint8_t message_num;
Motor_Type_e motor_type;
} dji_motor_instance;
dji_motor_instance* DJIMotorInit(CAN_HandleTypeDef* _hcan,uint16_t tx_id,uint16_t rx_id);
dji_motor_instance* DJIMotorInit(can_instance_config config,
Motor_Controller_s controller_config,
Motor_Control_Setting_s motor_setting,
Motor_Controller_Init_s controller_init,
Motor_Type_e type);
void DJIMotorSetRef();

52
modules/motor/motor_def.h
View File

@ -6,19 +6,65 @@ typedef enum
CURRENT=0,
SPEED=1,
ANGLE=2
} closeloop_e;
} Closeloop_Type_e;
typedef enum
{
MOTOR=0,
OTHER=1
} feedback_source_e;
} Feedback_Source_e;
typedef enum
{
CLOCKWISE=0,
COUNTER_CLOCKWISE=1
} reverse_flag_e;
} Reverse_Flag_e;
typedef struct
{
Closeloop_Type_e close_loop_type;
Reverse_Flag_e reverse_flag;
Feedback_Source_e angle_feedback_source;
Feedback_Source_e speed_feedback_source;
} Motor_Control_Setting_s;
typedef struct
{
float* other_angle_feedback_ptr;
float* other_speed_feedback_ptr;
PID_t* current_PID;
PID_t* speed_PID;
PID_t* angle_PID;
} Motor_Controller_s;
typedef enum
{
GM6020=0,
M3508=1,
M2006=2,
LK9025=3,
HT04=4
} Motor_Type_e;
typedef struct
{
/* data */
} PID_config_s;
typedef struct
{
float* other_angle_feedback_ptr;
float* other_speed_feedback_ptr;
PID_config_s current_PID;
PID_config_s speed_PID;
PID_config_s angle_PID;
} Motor_Controller_Init_s;

10
modules/motor/motor_task.h
View File

@ -4,15 +4,9 @@
#include "LK9025.h"
#include "HT04.h"
#include "dji_motor.h"
#include "motor_def.h"
typedef enum
{
GM6020=0,
M3508=1,
M2006=2,
LK9025=3,
HT04=4
} Motor_Type_e;
void MotorControlTask();