N3_sub/libraries/AP_UAVCAN/AP_UAVCAN.cpp

/*
 * This file is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This file is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Author: Eugene Shamaev, Siddharth Bharat Purohit
 */

#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>

#if HAL_WITH_UAVCAN
#include "AP_UAVCAN.h"


#include <GCS_MAVLink/GCS.h>
#include <AP_Motors/AP_Motors.h>  //电机类
#include "../../ArduSub/Sub.h"

#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>

#include <uavcan/transport/can_acceptance_filter_configurator.hpp>

#include <uavcan/equipment/actuator/ArrayCommand.hpp>
#include <uavcan/equipment/actuator/Command.hpp>
#include <uavcan/equipment/actuator/Status.hpp>

//#include <uavcan/equipment/esc/MotorCommands.hpp>
#include <uavcan/equipment/esc/Motor1ResCommand.hpp>
#include <uavcan/equipment/esc/SetFloatParam.hpp>
#include <uavcan/equipment/esc/SingleMotorCommand.hpp>
#include <uavcan/equipment/esc/GetFloatParam1.hpp>
//hight volt HighVoltThrusterSetRegs.hpp

#include <uavcan/equipment/esc/Bfd1.hpp>
#include <uavcan/equipment/esc/Bfd2.hpp>
#include <uavcan/equipment/esc/Bfd3.hpp>




#include <uavcan/equipment/esc/Clear.hpp>
#include <uavcan/equipment/esc/HighVoltThrusterSetRegs.hpp>

#include <uavcan/equipment/esc/HighVoltThruster1GetRegs.hpp>
#include <uavcan/equipment/esc/HighVoltThruster2GetRegs.hpp>
#include <uavcan/equipment/esc/HighVoltThruster3GetRegs.hpp>
#include <uavcan/equipment/esc/HighVoltThruster1GetRegsFujian.hpp>
#include <uavcan/equipment/esc/HighVoltThruster2GetRegsFujian.hpp>
#include <uavcan/equipment/esc/HighVoltThruster3GetRegsFujian.hpp>

#include <uavcan/equipment/esc/SpeedCommands.hpp>


#include <uavcan/equipment/esc/HighVoltThruster1timeback.hpp>
#include <uavcan/equipment/esc/HighVoltThruster2timeback.hpp>
#include <uavcan/equipment/esc/HighVoltThruster3timeback.hpp>

//#include <uavcan/equipment/esc/HighVoltThruster1timebackST.hpp>
//#include <uavcan/equipment/esc/HighVoltThruster2timebackST.hpp>
//#include <uavcan/equipment/esc/HighVoltThruster3timebackST.hpp>




#include <uavcan/equipment/esc/RawCommand.hpp>
#include <uavcan/equipment/indication/LightsCommand.hpp>
#include <uavcan/equipment/indication/SingleLightCommand.hpp>
#include <uavcan/equipment/indication/BeepCommand.hpp>
#include <uavcan/equipment/indication/RGB565.hpp>
#include <ardupilot/indication/SafetyState.hpp>
#include <ardupilot/indication/Button.hpp>

#include <AP_Baro/AP_Baro_UAVCAN.h>
#include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h>
#include <AP_GPS/AP_GPS_UAVCAN.h>
#include <AP_BattMonitor/AP_BattMonitor_UAVCAN.h>
#include <AP_Compass/AP_Compass_UAVCAN.h>
#include <AP_Airspeed/AP_Airspeed_UAVCAN.h>
#include <SRV_Channel/SRV_Channel.h>
#include <AP_OpticalFlow/AP_OpticalFlow_HereFlow.h>



#define LED_DELAY_US 50000

extern const AP_HAL::HAL& hal;

#define debug_uavcan(level_debug, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(_driver_index)) { printf(fmt, ##args); }} while (0)

// Translation of all messages from UAVCAN structures into AP structures is done
// in AP_UAVCAN and not in corresponding drivers.
// The overhead of including definitions of DSDL is very high and it is best to
// concentrate in one place.

// table of user settable CAN bus parameters
const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
    // @Param: NODE
    // @DisplayName: UAVCAN node that is used for this network
    // @Description: UAVCAN node should be set implicitly
    // @Range: 1 250
    // @User: Advanced
    AP_GROUPINFO("NODE", 1, AP_UAVCAN, _uavcan_node, 10),

    // @Param: SRV_BM
    // @DisplayName: RC Out channels to be transmitted as servo over UAVCAN
    // @Description: Bitmask with one set for channel to be transmitted as a servo command over UAVCAN
    // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15
    // @User: Advanced
    AP_GROUPINFO("SRV_BM", 2, AP_UAVCAN, _servo_bm, 0),

    // @Param: ESC_BM
    // @DisplayName: RC Out channels to be transmitted as ESC over UAVCAN
    // @Description: Bitmask with one set for channel to be transmitted as a ESC command over UAVCAN
    // @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16
    // @User: Advanced
    AP_GROUPINFO("ESC_BM", 3, AP_UAVCAN, _esc_bm, 0),

    // @Param: SRV_RT
    // @DisplayName: Servo output rate
    // @Description: Maximum transmit rate for servo outputs
    // @Range: 1 200
    // @Units: Hz
    // @User: Advanced
    AP_GROUPINFO("SRV_RT", 4, AP_UAVCAN, _servo_rate_hz, 50),

    AP_GROUPEND
};

// this is the timeout in milliseconds for periodic message types. We
// set this to 1 to minimise resend of stale msgs
#define CAN_PERIODIC_TX_TIMEOUT_MS 2

//自定义stm32 参数设置协议--------
static uavcan::Publisher<uavcan::equipment::esc::HighVoltThrusterSetRegs>* HVoltThtSetReg[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::esc::Bfd1>* esc_Bfd1[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::esc::Bfd2>* esc_Bfd2[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::esc::Bfd3>* esc_Bfd3[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::esc::Clear>* clearFault[MAX_NUMBER_OF_CAN_DRIVERS];





//static uavcan::Publisher<uavcan::equipment::esc::MotorCommands>* esc_MotorCommands[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::esc::SetFloatParam>* SetFloatParam[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::esc::SpeedCommands>* esc_SpeedCommands[MAX_NUMBER_OF_CAN_DRIVERS];




//自定义stm32 参数设置协议--------

// publisher interfaces
static uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>* act_out_array[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::esc::RawCommand>* esc_raw[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[MAX_NUMBER_OF_CAN_DRIVERS];

// subscribers
// handler SafteyButton
UC_REGISTRY_BINDER(ButtonCb, ardupilot::indication::Button);
static uavcan::Subscriber<ardupilot::indication::Button, ButtonCb> *safety_button_listener[MAX_NUMBER_OF_CAN_DRIVERS];

//接收局部全局变量-----------
int16_t motors_receive[12]={0,0,0,0,0,0,0,0,0,0,0,0}; //STM32发来的速度
uint8_t crc_motors[42];

union DRIVEBOX_ABNORMAL_UNI motor_stall_state;//获取所有的堵转



union PROPELLER_ABNORMAL_UNI propeller_block;//推进器缠绕等级
union PROPELLER_ABNORMAL_UNI track_block;//推进器缠绕等级


int16_t temperature_power = 0;//温度
int16_t voltage48V= 0;//48V电源
//int16_t driveleak = 0;//泄露

extern mavlink_set_slave_parameter_t set_stm32_param;

extern mavlink_rov_state_monitoring_t rov_message;

extern mavlink_hv_reg_get_t hv_reg_get;
extern mavlink_hv_reg_set_t hv_reg_set;
extern mavlink_motor_speed_t mav_motor_speed;
//uint8_t mode_flag = 0;

uint8_t get_stm32_param_buf[7] = {0,0,0,0,0,0,0};// 板1收到的参数



//hv_motor_runmode.all = 0xFFFF;
uint8_t hv_motor_cmd_temp = 0;
uint8_t hv_motor_cmd = 0;
int16_t hv_motor_speed_32 = 0;


int16_t lightstate=0;
int16_t usbl_power=0;
int16_t light_leak=0;

#define reboot_time 3000
//----------------------------------------------
float getfloat(mavlink_set_slave_parameter_t par);
void STM32_set_packet(uavcan::equipment::esc::SetFloatParam &_msgparam);
void crc_setmotor(uavcan::equipment::esc::HighVoltThrusterSetRegs &hvmesg);
void crc_ackfault(uavcan::equipment::esc::HighVoltThrusterSetRegs &hvmesg,uint8_t number);


AP_UAVCAN::AP_UAVCAN() :
    _node_allocator()
{
    AP_Param::setup_object_defaults(this, var_info);
	_singleton = this;

    for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
        _SRV_conf[i].esc_pending = false;
        _SRV_conf[i].servo_pending = false;
    }
	sublightstate = 0;
	trackblock_flag =0;
	propellerblock_flag =0;
	motor_stall_state.all = 0;
	propeller_block.all = 0;
	track_block.all = 0;
	for (uint8_t cnt = 0; cnt < 12; cnt++)
	{
		motors_receive[cnt]=0;
	}
	for(uint8_t j=0;j<12;j++){
	motor_from_stm32[j] = 0;//读到转速
	}

	

    debug_uavcan(2, "AP_UAVCAN constructed\n\r");
}

AP_UAVCAN::~AP_UAVCAN()
{
}

AP_UAVCAN *AP_UAVCAN::get_uavcan(uint8_t driver_index)
{
    if (driver_index >= AP::can().get_num_drivers() ||
        AP::can().get_protocol_type(driver_index) != AP_BoardConfig_CAN::Protocol_Type_UAVCAN) {
        return nullptr;
    }
    return static_cast<AP_UAVCAN*>(AP::can().get_driver(driver_index));
}
//self define rec fun-------------------------------start---------------
static void motor_res_st_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Motor1ResCommand>& msg,uint8_t mgr )
{//接收到的数据在这里处理，打印出来

	if (msg.motor_index == 0){
		//推进器0，1，2
		motors_receive[0] = msg.rpm0 & 0x0FFF;//推进器0
		propeller_block.bit.motor0 = (msg.rpm0 & 0xF000)>>12;
		motors_receive[1] = msg.rpm1 & 0x0FFF;//推进器1
		propeller_block.bit.motor1 = (msg.rpm1 & 0xF000)>>12;
		
	}
	else if(msg.motor_index == 3){
		//推进器0.1.履带左的给定
		int16_t rmp1  = msg.rpm0;
		int16_t rmp2 =  msg.rpm1;
		int16_t rmp3 =  msg.rpm2;

		static int16_t c2  = 0;
		c2++;
		if(c2>50){
			c2 =0 ;
			gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 3 %d %d %d.",(int)rmp1,(int)rmp2,(int)rmp3);
				
		}

	}
	else if(msg.motor_index == 6){
		
		
		int16_t rpm = msg.rpm0;//履带右的给定
		

		motors_receive[8] = msg.rpm2 & 0x0FFF;//履带1
		track_block.bit.motor0 = (msg.rpm2 & 0xF000)>>12;

		static int16_t c3  = 0;
		c3++;
		if(c3>50){
			c3 =0 ;
			gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 6 %d %d ",(int)rpm,motors_receive[8]);
			
		}
	}
	else if(msg.motor_index == 9){
	////履带2
		motors_receive[9] = msg.rpm0 & 0x0FFF;//履带2速度
		
		track_block.bit.motor1 = (msg.rpm0 & 0xF000)>>12;
		
		static int16_t c4  = 0;
		c4++;
		if(c4>100){
			c4 =0 ;
			gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 9 %d.",(int)motors_receive[9]);
			
		}
	}
	else if(msg.motor_index == 12){
	////
		voltage48V = msg.rpm0;
		temperature_power = msg.rpm1;
		motor_stall_state.all = msg.rpm2;//所有电机的堵转// 履带没有堵转了

		static int16_t c5  = 0;
		c5++;
		if(c5>100){
			c5 =0 ;
			//gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 12 %d %d %x.",(int)voltage48V,(int)temperature_power,motor_stall_state.all);
			
		}
	}
	else if(msg.motor_index == 15){
	////履带2,毛刷1,毛刷2
		lightstate = msg.rpm0;
		usbl_power = msg.rpm1;
		light_leak = msg.rpm2;//灯 漏水


	}
}

static void motor_res_st_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Motor1ResCommand>& msg ){
	motor_res_st_cb(msg,0);
}
static void motor_res_st_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Motor1ResCommand>& msg ){
	motor_res_st_cb(msg,1);
}


static void (*motor_res_st_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Motor1ResCommand>& msg )
		={motor_res_st_cb0,motor_res_st_cb1};

//==========================================================================================================================
//获取stm32板1参数------XXXXXXXX.YY-------------------------
float getfloat(mavlink_set_slave_parameter_t par){
		float data =(par.XX1/16)*10000000+(par.XX1%16)*1000000 + 
					(par.XX2/16)*100000+(par.XX2%16)*10000+
					(par.XX3/16)*1000+(par.XX3%16)*100 +
					(par.XX4/16)*10 + (par.XX4%16)	+ 
					(par.YY/16)*0.1 +(par.YY%16)*0.01;
		return data;
}

static void motor_par1_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::GetFloatParam1>& msg,uint8_t mgr ){
	get_stm32_param_buf[0] = msg.number;
	get_stm32_param_buf[1] = msg.flag;
	get_stm32_param_buf[2] = msg.XX1;
	get_stm32_param_buf[3] = msg.XX2;
	get_stm32_param_buf[4] = msg.XX3;
	get_stm32_param_buf[5] = msg.XX4;
	get_stm32_param_buf[6] = msg.YY;

	
	
	
}
static void motor_par1_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::GetFloatParam1>& msg ){
			motor_par1_cb(msg,0);
}
static void motor_par1_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::GetFloatParam1>& msg ){
			motor_par1_cb(msg,1);
}

static void (*motor_par1_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::GetFloatParam1>& msg )
		={motor_par1_cb0,motor_par1_cb1};

//高压电调1参数返回
		static void thruster1_1GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg,uint8_t mgr ){
			uint8_t buffer[7]={0,0,0,0,0,0,0};
			
			for (uint8_t i = 0; i < 7; i++)
			{
				buffer[i] = msg.BUFFER[i];
			}
		
			hv_reg_get.aim = 3;
			hv_reg_get.data[0] = (uint32_t)buffer[1] + (uint32_t)(buffer[2]<<8) + (uint32_t)(buffer[3]<<16) + (uint32_t)(buffer[4]<<24);
			hv_reg_get.data[1] = 0;
			hv_reg_get.data[2] = 0;
			
			
		
			gcs().send_text(MAV_SEVERITY_WARNING, "thruster3 %x .",(uint32_t)hv_reg_get.data[0]);
		}
		static void Thruster1_1GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg ){
					thruster1_1GetRegs_cb(msg,0);
		}
		static void Thruster1_1GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg ){
					thruster1_1GetRegs_cb(msg,1);
		}
		
		static void (*Thruster1_1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg )
				={Thruster1_1GetRegs_cb0,Thruster1_1GetRegs_cb1};
//福建
static void thruster1GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegsFujian>& msg,uint8_t mgr ){
	uint8_t buffer[7]={0,0,0,0,0,0,0};
	
	for (uint8_t i = 0; i < 7; i++)
	{
		buffer[i] = msg.BUFFER[i];
	}

	hv_reg_get.aim = 3;
	hv_reg_get.data[0] = (uint32_t)buffer[1] + (uint32_t)(buffer[2]<<8) + (uint32_t)(buffer[3]<<16) + (uint32_t)(buffer[4]<<24);
	hv_reg_get.data[1] = 0;
	hv_reg_get.data[2] = 0;
	
	

	//gcs().send_text(MAV_SEVERITY_WARNING, "thruster3 %x .",(uint32_t)hv_reg_get.data[0]);
}
static void Thruster1GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegsFujian>& msg ){
			thruster1GetRegs_cb(msg,0);
}
static void Thruster1GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegsFujian>& msg ){
			thruster1GetRegs_cb(msg,1);
}

static void (*Thruster1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegsFujian>& msg )
		={Thruster1GetRegs_cb0,Thruster1GetRegs_cb1};

//高压电调2参数返回
static void thruster2_1GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg ,uint8_t mgr){

	uint8_t buffer[7]={0,0,0,0,0,0,0};
	
	for (uint8_t i = 0; i < 7; i++)
	{
		buffer[i] = msg.BUFFER[i];
	}

	hv_reg_get.aim = 4;
	
	hv_reg_get.data[0] = (uint32_t)buffer[1] + (uint32_t)(buffer[2]<<8) + (uint32_t)(buffer[3]<<16) + (uint32_t)(buffer[4]<<24);
	hv_reg_get.data[1] = 0;
	hv_reg_get.data[2] = 0;
	
	

	gcs().send_text(MAV_SEVERITY_WARNING, "thruster4 %x .",(uint32_t)hv_reg_get.data[0]);



}
static void Thruster2_1GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg ){
			thruster2_1GetRegs_cb(msg,0);
}
static void Thruster2_1GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg ){
			thruster2_1GetRegs_cb(msg,1);
}

static void (*Thruster2_1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg )
		={Thruster2_1GetRegs_cb0,Thruster2_1GetRegs_cb1};
//福建
static void thruster2GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegsFujian>& msg ,uint8_t mgr){

	uint8_t buffer[7]={0,0,0,0,0,0,0};
	
	for (uint8_t i = 0; i < 7; i++)
	{
		buffer[i] = msg.BUFFER[i];
	}

	hv_reg_get.aim = 4;
	
	hv_reg_get.data[0] = (uint32_t)buffer[1] + (uint32_t)(buffer[2]<<8) + (uint32_t)(buffer[3]<<16) + (uint32_t)(buffer[4]<<24);
	hv_reg_get.data[1] = 0;
	hv_reg_get.data[2] = 0;
	
	

	//gcs().send_text(MAV_SEVERITY_WARNING, "thruster4 %x .",(uint32_t)hv_reg_get.data[0]);



}
static void Thruster2GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegsFujian>& msg ){
			thruster2GetRegs_cb(msg,0);
}
static void Thruster2GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegsFujian>& msg ){
			thruster2GetRegs_cb(msg,1);
}

static void (*Thruster2GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegsFujian>& msg )
		={Thruster2GetRegs_cb0,Thruster2GetRegs_cb1};
//高压电调3参数返回
static void thruster3_1GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& msg ,uint8_t mgr){
	uint8_t buffer[7]={0,0,0,0,0,0,0};
	
	for (uint8_t i = 0; i < 7; i++)
	{
		buffer[i] = msg.BUFFER[i];
	}

	hv_reg_get.aim = 5;
	
	hv_reg_get.data[0] = (uint32_t)buffer[1] + (uint32_t)(buffer[2]<<8) + (uint32_t)(buffer[3]<<16) + (uint32_t)(buffer[4]<<24);
	hv_reg_get.data[1] = 0;
	hv_reg_get.data[2] = 0;
	
	

	gcs().send_text(MAV_SEVERITY_WARNING, "thruster5 %x .",(uint32_t)hv_reg_get.data[0]);


	
	//gcs().send_text(MAV_SEVERITY_WARNING, "thruster3data %d %d %d %d.",(int)buffer[0],(int)buffer[1],(int)buffer[2],(int)buffer[3]);

	//gcs().send_text(MAV_SEVERITY_WARNING, "thruster3 %d %d %d.",hv_reg_get.aim,(int)data[0],(int)data[1]);
}
static void Thruster3_1GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& msg ){
			thruster3_1GetRegs_cb(msg,0);
}
static void Thruster3_1GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& msg ){
			thruster3_1GetRegs_cb(msg,1);
}

static void (*Thruster3_1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& msg )
		={Thruster3_1GetRegs_cb0,Thruster3_1GetRegs_cb1};
//福建
static void thruster3GetRegs_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegsFujian>& msg ,uint8_t mgr){
	uint8_t buffer[7]={0,0,0,0,0,0,0};
	
	for (uint8_t i = 0; i < 7; i++)
	{
		buffer[i] = msg.BUFFER[i];
	}

	hv_reg_get.aim = 5;
	
	hv_reg_get.data[0] = (uint32_t)buffer[1] + (uint32_t)(buffer[2]<<8) + (uint32_t)(buffer[3]<<16) + (uint32_t)(buffer[4]<<24);
	hv_reg_get.data[1] = 0;
	hv_reg_get.data[2] = 0;
	
	

	//gcs().send_text(MAV_SEVERITY_WARNING, "thruster5 %x .",(uint32_t)hv_reg_get.data[0]);


	
	//gcs().send_text(MAV_SEVERITY_WARNING, "thruster3data %d %d %d %d.",(int)buffer[0],(int)buffer[1],(int)buffer[2],(int)buffer[3]);

	//gcs().send_text(MAV_SEVERITY_WARNING, "thruster3 %d %d %d.",hv_reg_get.aim,(int)data[0],(int)data[1]);
}
static void Thruster3GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegsFujian>& msg ){
			thruster3GetRegs_cb(msg,0);
}
static void Thruster3GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegsFujian>& msg ){
			thruster3GetRegs_cb(msg,1);
}

static void (*Thruster3GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegsFujian>& msg )
		={Thruster3GetRegs_cb0,Thruster3GetRegs_cb1};

//高压电调1定时返回
struct HVmes Thruster1={0,0,0,0,0,0};
struct HVmes Thruster2={0,0,0,0,0,0};
struct HVmes Thruster3={0,0,0,0,0,0};


struct HVmotor haoye1={0,0,0,0,0,0};
struct HVmotor haoye2={0,0,0,0,0,0};

//static void Thruster1timeback_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1timebackST>& msg ,uint8_t mgr){
	static void Thruster1timeback_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1timeback>& msg ,uint8_t mgr){
	
	uint8_t buffer[6]={0,0,0,0,0,0};
	for (uint8_t i = 0; i < 6; i++)
	{
		buffer[i] = msg.BUFFER[i];
	}
	HVcycle(msg.CODE,buffer,Thruster1);
	
	//if(msg.CODE == 0x0A){
		
		//int16_t speedmes = buffer[4]+(buffer[5]<<8);
		static int16_t per = 0;
		per++;
		if (per>150)
		{
			per = 0;
			gcs().send_text(MAV_SEVERITY_INFO, "HV3 %d %d %d %d %d",(int)Thruster1.voltage,(int)Thruster1.speedref,(int)Thruster1.temperature,(int)Thruster1.currentlimit,(int)Thruster1.Imax/336);
			}
			
	//}


	
}
//static void Thruster1timeback_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1timebackST>& msg ){
static void Thruster1timeback_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1timeback>& msg ){

			Thruster1timeback_cb(msg,0);
}
//static void Thruster1timeback_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1timebackST>& msg ){
static void Thruster1timeback_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1timeback>& msg ){

			Thruster1timeback_cb(msg,1);
}

//static void (*Thruster1timeback_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1timebackST>& msg )
static void (*Thruster1timeback_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1timeback>& msg )

		={Thruster1timeback_cb0,Thruster1timeback_cb1};
//高压电调2定时返回
//static void Thruster2timeback_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2timebackST>& msg ,uint8_t mgr){
static void Thruster2timeback_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2timeback>& msg ,uint8_t mgr){
		
	uint8_t buffer[6]={0,0,0,0,0,0};
	for (uint8_t i = 0; i < 6; i++)
	{
		buffer[i] = msg.BUFFER[i];
	}
	HVcycle(msg.CODE,buffer,Thruster2);
	
	//if(msg.CODE == 0x0A){
		
		//int16_t speedmes = buffer[4]+(buffer[5]<<8);
		static int16_t per = 0;
		per++;
		if (per>150)
		{
			per = 0;
			gcs().send_text(MAV_SEVERITY_INFO, "HV4 %d %d %d %d %d",(int)Thruster2.voltage,(int)Thruster2.speedref,(int)Thruster2.temperature,(int)Thruster2.currentlimit,(int)Thruster2.Imax/336);
		}
			
	//}
	

}
//static void Thruster2timeback_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2timebackST>& msg ){
static void Thruster2timeback_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2timeback>& msg ){

			Thruster2timeback_cb(msg,0);
}
//static void Thruster2timeback_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2timebackST>& msg ){
static void Thruster2timeback_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2timeback>& msg ){

			Thruster2timeback_cb(msg,1);
}

//static void (*Thruster2timeback_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2timebackST>& msg )
static void (*Thruster2timeback_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2timeback>& msg )

		={Thruster2timeback_cb0,Thruster2timeback_cb1};

//高压电调2定时返回
//static void Thruster3timeback_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3timebackST>& msg ,uint8_t mgr){
static void Thruster3timeback_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3timeback>& msg ,uint8_t mgr){		
	uint8_t buffer[6]={0,0,0,0,0,0};
	for (uint8_t i = 0; i < 6; i++)
	{
		buffer[i] = msg.BUFFER[i];
	}
	HVcycle(msg.CODE,buffer,Thruster3);
	
		//if(msg.CODE == 0x0A){
		
		//int16_t speedmes = buffer[4]+(buffer[5]<<8);
		static int16_t per = 0;
		per++;
		if (per>150)
		{
			per = 0;
			gcs().send_text(MAV_SEVERITY_INFO, "HV5 %d %d %d %d %d",(int)Thruster3.voltage,(int)Thruster3.speedref,(int)Thruster3.temperature,(int)Thruster3.currentlimit,(int)Thruster3.Imax/336);
		}
			
	//}

}
//static void Thruster3timeback_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3timebackST>& msg ){
static void Thruster3timeback_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3timeback>& msg ){

			Thruster3timeback_cb(msg,0);
}
//static void Thruster3timeback_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3timebackST>& msg ){
static void Thruster3timeback_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3timeback>& msg ){

			Thruster3timeback_cb(msg,1);
}

//static void (*Thruster3timeback_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3timebackST>& msg )
static void (*Thruster3timeback_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3timeback>& msg )

		={Thruster3timeback_cb0,Thruster3timeback_cb1};



//self define rec fun-------------------------------end---------------


void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
{
	//hal.scheduler->delay_microseconds(1000);

    if (_initialized) {
        debug_uavcan(2, "UAVCAN: init called more than once\n\r");
        return;
    }

    _driver_index = driver_index;

    AP_HAL::CANManager* can_mgr = hal.can_mgr[driver_index];
    if (can_mgr == nullptr) {
        debug_uavcan(2, "UAVCAN: init called for inexisting CAN driver\n\r");
        return;
    }

    if (!can_mgr->is_initialized()) {
        debug_uavcan(1, "UAVCAN: CAN driver not initialized\n\r");
        return;
    }

    uavcan::ICanDriver* driver = can_mgr->get_driver();
    if (driver == nullptr) {
        debug_uavcan(2, "UAVCAN: can't get UAVCAN interface driver\n\r");
        return;
    }

    _node = new uavcan::Node<0>(*driver, SystemClock::instance(), _node_allocator);

    if (_node == nullptr) {
        debug_uavcan(1, "UAVCAN: couldn't allocate node\n\r");
        return;
    }

    if (_node->isStarted()) {
        debug_uavcan(2, "UAVCAN: node was already started?\n\r");
        return;
    }

    uavcan::NodeID self_node_id(_uavcan_node);
    _node->setNodeID(self_node_id);

    char ndname[20];
    snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index);

    uavcan::NodeStatusProvider::NodeName name(ndname);
    _node->setName(name);

    uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion
    sw_version.major = AP_UAVCAN_SW_VERS_MAJOR;
    sw_version.minor = AP_UAVCAN_SW_VERS_MINOR;
    _node->setSoftwareVersion(sw_version);

    uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion

    hw_version.major = AP_UAVCAN_HW_VERS_MAJOR;
    hw_version.minor = AP_UAVCAN_HW_VERS_MINOR;

    const uint8_t uid_buf_len = hw_version.unique_id.capacity();
    uint8_t uid_len = uid_buf_len;
    uint8_t unique_id[uid_buf_len];

    if (hal.util->get_system_id_unformatted(unique_id, uid_len)) {
        uavcan::copy(unique_id, unique_id + uid_len, hw_version.unique_id.begin());
    }
    _node->setHardwareVersion(hw_version);

    int start_res = _node->start();
    if (start_res < 0) {
        debug_uavcan(1, "UAVCAN: node start problem, error %d\n\r", start_res);
        return;
    }

    //Start Servers
#ifdef HAS_UAVCAN_SERVERS
    _servers.init(*_node);
#endif
    // Roundup all subscribers from supported drivers
    AP_GPS_UAVCAN::subscribe_msgs(this);
    AP_Compass_UAVCAN::subscribe_msgs(this);
    AP_Baro_UAVCAN::subscribe_msgs(this);
    AP_BattMonitor_UAVCAN::subscribe_msgs(this);
    AP_Airspeed_UAVCAN::subscribe_msgs(this);
    AP_OpticalFlow_HereFlow::subscribe_msgs(this);
    AP_RangeFinder_UAVCAN::subscribe_msgs(this);
	
	//创建设备类型------------------
	 auto *node= get_node();
	uavcan::Subscriber<uavcan::equipment::esc::Motor1ResCommand>* motor_res_st;
	motor_res_st = new uavcan::Subscriber<uavcan::equipment::esc::Motor1ResCommand>(*node);
	const int motor_res_start_res = motor_res_st->start(motor_res_st_cb_arr[_driver_index]);//定义回调函数用于接收
	if (motor_res_start_res<0)
	{
		return;
	}


	//创建设备类型获取stm32参数1

	uavcan::Subscriber<uavcan::equipment::esc::GetFloatParam1>* motor_par1_st;
	motor_par1_st = new uavcan::Subscriber<uavcan::equipment::esc::GetFloatParam1>(*node);
	const int motor_par1_res = motor_par1_st->start(motor_par1_cb_arr[_driver_index]);//定义回调函数用于接收
	if (motor_par1_res<0)
	{
		return;
	}
		//高压电调1 参数返回------------------------------------------------------------
	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1GetRegs>* Thruster1_1GetRegs;
	Thruster1_1GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1GetRegs>(*node);
	const int Tht1_1GetReg = Thruster1_1GetRegs->start(Thruster1_1GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
	if (Tht1_1GetReg<0)
	{
		return;
	}	

	//高压电调2 参数返回
	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster2GetRegs>* Thruster2_1GetRegs;
	Thruster2_1GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster2GetRegs>(*node);
	const int Tht2_1GetReg = Thruster2_1GetRegs->start(Thruster2_1GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
	if (Tht2_1GetReg<0)
	{
		return;
	}

	//高压电调3 参数返回
	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster3GetRegs>* Thruster3_1GetRegs;
	Thruster3_1GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster3GetRegs>(*node);
	const int Tht3_1GetReg = Thruster3_1GetRegs->start(Thruster3_1GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
	if (Tht3_1GetReg<0)
	{
		return;
	}//以下是福建的--------------------------
	//高压电调1 参数返回
	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1GetRegsFujian>* Thruster1GetRegs;
	Thruster1GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1GetRegsFujian>(*node);
	const int Tht1GetReg = Thruster1GetRegs->start(Thruster1GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
	if (Tht1GetReg<0)
	{
		return;
	}	

	//高压电调2 参数返回
	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster2GetRegsFujian>* Thruster2GetRegs;
	Thruster2GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster2GetRegsFujian>(*node);
	const int Tht2GetReg = Thruster2GetRegs->start(Thruster2GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
	if (Tht2GetReg<0)
	{
		return;
	}

	//高压电调3 参数返回
	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster3GetRegsFujian>* Thruster3GetRegs;
	Thruster3GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster3GetRegsFujian>(*node);
	const int Tht3GetReg = Thruster3GetRegs->start(Thruster3GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
	if (Tht3GetReg<0)
	{
		return;
	}



		//高压电调1 定时参数返回
	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1timeback>* Thruster1timeback;
	Thruster1timeback = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1timeback>(*node);
	const int Tht1timecb = Thruster1timeback->start(Thruster1timeback_cb_arr[_driver_index]);//定义回调函数用于接收
	if (Tht1timecb<0)
	{
		return;
	}
	//高压电调2 定时参数返回
	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster2timeback>* Thruster2timeback;
	Thruster2timeback = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster2timeback>(*node);
	const int Tht2timecb = Thruster2timeback->start(Thruster2timeback_cb_arr[_driver_index]);//定义回调函数用于接收
	if (Tht2timecb<0)
	{
		return;
	}

		//高压电调3 定时参数返回
	uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster3timeback>* Thruster3timeback;
	Thruster3timeback = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster3timeback>(*node);
	const int Tht3timecb = Thruster3timeback->start(Thruster3timeback_cb_arr[_driver_index]);//定义回调函数用于接收
	if (Tht3timecb<0)
	{
		return;
	}
	//创建发送设备类型


	
	esc_SpeedCommands[driver_index] = new uavcan::Publisher<uavcan::equipment::esc::SpeedCommands>(*_node);
    esc_SpeedCommands[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
    esc_SpeedCommands[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);


	//HVoltThtSetReg
	//高压电调清故障
	clearFault[driver_index] = new uavcan::Publisher<uavcan::equipment::esc::Clear>(*_node);
    clearFault[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
    clearFault[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);	
	//高压电调参数设置
	HVoltThtSetReg[driver_index] = new uavcan::Publisher<uavcan::equipment::esc::HighVoltThrusterSetRegs>(*_node);
    HVoltThtSetReg[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
    HVoltThtSetReg[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);

	
	//创建发送设备类型
   esc_Bfd1[driver_index] = new uavcan::Publisher<uavcan::equipment::esc::Bfd1>(*_node);
   esc_Bfd1[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
   esc_Bfd1[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);

   esc_Bfd2[driver_index] = new uavcan::Publisher<uavcan::equipment::esc::Bfd2>(*_node);
   esc_Bfd2[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
   esc_Bfd2[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);


   esc_Bfd3[driver_index] = new uavcan::Publisher<uavcan::equipment::esc::Bfd3>(*_node);
   esc_Bfd3[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
   esc_Bfd3[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
	//esc_MotorCommands[driver_index] = new uavcan::Publisher<uavcan::equipment::esc::MotorCommands>(*_node);
   // esc_MotorCommands[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
   // esc_MotorCommands[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
	
	//创建参数设置类型
	SetFloatParam[driver_index] = new uavcan::Publisher<uavcan::equipment::esc::SetFloatParam>(*_node);
	SetFloatParam[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
	SetFloatParam[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);

	//-----------------------------------

    act_out_array[driver_index] = new uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>(*_node);
    act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
    act_out_array[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);

    esc_raw[driver_index] = new uavcan::Publisher<uavcan::equipment::esc::RawCommand>(*_node);
    esc_raw[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
    esc_raw[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);

    rgb_led[driver_index] = new uavcan::Publisher<uavcan::equipment::indication::LightsCommand>(*_node);
    rgb_led[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
    rgb_led[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);

    buzzer[driver_index] = new uavcan::Publisher<uavcan::equipment::indication::BeepCommand>(*_node);
    buzzer[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
    buzzer[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);

    safety_state[driver_index] = new uavcan::Publisher<ardupilot::indication::SafetyState>(*_node);
    safety_state[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
    safety_state[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);

    safety_button_listener[driver_index] = new uavcan::Subscriber<ardupilot::indication::Button, ButtonCb>(*_node);
    if (safety_button_listener[driver_index]) {
        safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button));
    }

    _led_conf.devices_count = 0;
    if (enable_filters) {
        configureCanAcceptanceFilters(*_node);
    }

    /*
     * Informing other nodes that we're ready to work.
     * Default mode is INITIALIZING.
     */
    _node->setModeOperational();

    // Spin node for device discovery
    _node->spin(uavcan::MonotonicDuration::fromMSec(5000));

    snprintf(_thread_name, sizeof(_thread_name), "uavcan_%u", driver_index);

    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UAVCAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_CAN, 0)) {
        _node->setModeOfflineAndPublish();
        debug_uavcan(1, "UAVCAN: couldn't create thread\n\r");
        return;
    }

    _initialized = true;
    debug_uavcan(2, "UAVCAN: init done\n\r");
}


void STM32_set_packet(uavcan::equipment::esc::SetFloatParam &_msgparam){
	_msgparam.number =set_stm32_param.number;
	_msgparam.flag = set_stm32_param.flag;
	_msgparam.XX1 = set_stm32_param.XX1;
	_msgparam.XX2 = set_stm32_param.XX2;
	_msgparam.XX3 = set_stm32_param.XX3;
	_msgparam.XX4 = set_stm32_param.XX4;
	_msgparam.YY  = set_stm32_param.YY;

}

void crc_setmotor(uavcan::equipment::esc::HighVoltThrusterSetRegs &hvmesg){
	uint8_t crc_dataset[10]={0,0,0,0,0,0,0,0,0,0};

	
	hvmesg.NUMBER = hv_reg_set.aim;
	
	hvmesg.TYPE = hv_reg_set.code; 
	hvmesg.PARNUMBER	  = (uint16_t)hv_reg_set.data[0] + (uint16_t)(hv_reg_set.data[1]<<8);
	crc_dataset[0] = hvmesg.NUMBER ;
	crc_dataset[1] = hvmesg.TYPE;
	crc_dataset[2] = hv_reg_set.data[0];
	crc_dataset[3] = hv_reg_set.data[1];
	for (uint8_t j = 0; j < 6; j++)
	{
		hvmesg.DATA[j] = hv_reg_set.data[j+2];
		crc_dataset[j+4] = hvmesg.DATA[j];
	}
	uint16_t crcdata = calc_crc_modbus(crc_dataset,10);
	hvmesg.DATA[6] = (uint8_t)crcdata;
	hvmesg.DATA[7] = (uint8_t)(crcdata>>8);

}
void crc_ackfault(uavcan::equipment::esc::HighVoltThrusterSetRegs &hvmesg,uint8_t number){
	uint8_t crc_dataset[10]={0,0,0,0,0,0,0,0,0,0};

	hvmesg.NUMBER = number;
	hvmesg.TYPE = 0x03; 
	hvmesg.PARNUMBER	  = 0x5A5A;
	crc_dataset[0] = hvmesg.NUMBER ;
	crc_dataset[1] = hvmesg.TYPE;
	crc_dataset[2] = 0x5A;
	crc_dataset[3] = 0x5A;
	for (uint8_t j = 0; j < 6; j++)
	{
		hvmesg.DATA[j] = 0;
		crc_dataset[j+4] = hvmesg.DATA[j];
	}
	uint16_t crcdata = calc_crc_modbus(crc_dataset,10);
	hvmesg.DATA[6] = (uint8_t)crcdata;
	hvmesg.DATA[7] = (uint8_t)(crcdata>>8);

}

uint8_t HVreset = 0;


void AP_UAVCAN::loop(void)
{
    while (true) {
        if (!_initialized) {
            hal.scheduler->delay_microseconds(1000);
            continue;
        }

        const int error = _node->spin(uavcan::MonotonicDuration::fromMSec(1));

        if (error < 0) {
            hal.scheduler->delay_microseconds(100);
            continue;
        }
	//接收的周期性数据--------------------

			for(uint8_t j=0;j<12;j++){
				motor_from_stm32[j] = motors_receive[j];//读到转速
			}
			motor_stall_flag = motor_stall_state.all;//堵转

			
			propellerblock_flag = propeller_block.all;//推进器缠绕等级
			trackblock_flag = track_block.all;//推进器缠绕等级

			
			temperature_48Vpower = temperature_power;//can获取18B20温度
			board_voltage = voltage48V;//can获取48V电源
			driverleakstate = light_leak;//灯 漏水
			sublightstate = lightstate;

			HVmotor1 = Thruster1;
			HVmotor2 = Thruster2;
			HVmotor3 = Thruster3;

			rov_message.hvMotorMod = 0xffff;
		
		
		uavcan::equipment::esc::HighVoltThrusterSetRegs hvmesg;//高压电调参数设置
		

		//2-------------------aim取值0，1，2，3  高压电调设置
		if(hv_reg_set.aim == 0 || (hv_reg_set.aim &0x0F) !=0){
			crc_setmotor(hvmesg);
			
			hv_reg_set.aim = 0xF0;
			
			
			HVoltThtSetReg[_driver_index]->broadcast(hvmesg);//高压电调读取或者设置参数
			}
		//----------------------------------------------------------
		//2---------------高压电调设置end*******************************************

			
	//---------重启设置---------------------------
	
	static uint8_t stm32_rboot = 0;
	
		
		
		if((set_stm32_param.flag == 0xF0)&&(set_stm32_param.number ==12)){

			set_stm32_param.flag = 0;
			//消除缠绕
			stm32_rboot = 1;
			//高压电调消除故障
			HVreset = 7;	
			uavcan::equipment::esc::Clear Clearesg;//高压电调参数设置
			clearFault[_driver_index]->broadcast(Clearesg);//高压电调读取或者设置参数
		}
		

	//-------发送数据-------------------------------------------------

		const AP_Motors6DOF &motors6dof = AP::motors6dof();//6自由度电机计算出来的PWM
		uavcan::equipment::esc::SingleMotorCommand msg;
		uavcan::equipment::esc::SpeedCommands msgarryspeed;
		uavcan::equipment::esc::SetFloatParam msgparam;	
		

		uavcan::equipment::esc::Bfd1 msgbfd1;
		uavcan::equipment::esc::Bfd2 msgbfd2;
		uavcan::equipment::esc::Bfd3 msgbfd3;
		

		uint32_t nowtime = AP_HAL::micros();
		static uint32_t last_send_time = nowtime;
		if (nowtime -  last_send_time >1000000UL/200 )//发送周期  
		{   last_send_time = nowtime;
			
			msgbfd1.index = 0;
			msgbfd1.speed1 = 0;
			msgbfd1.speed2 = 0;
			if ((HVreset&0x01)==1 )
			{
				if (HVmotor1.fault!=0)
				{
					msgbfd1.speed3 = 0;
				}else{
					msgbfd1.speed3 = motors6dof.motor_to_can[2];
					HVreset = HVreset&0x06;
				}
			}else{
				msgbfd1.speed3 = motors6dof.motor_to_can[2];
			}
			
			esc_Bfd1[_driver_index]->broadcast(msgbfd1);//大约100HZ 在没有设置参数的情况下 发送驱动*/
		//-----------------------------------------------------------------------------------------------------
			msgbfd2.index = 0;
			if ((HVreset&0x02)==2 )
			{
				if (HVmotor2.fault!=0)
				{
					msgbfd2.speed1 = 0;
					
				}else{
					msgbfd2.speed1 = motors6dof.motor_to_can[3];
					HVreset =HVreset&0x05;
					
				}
			}else{
				msgbfd2.speed1 = motors6dof.motor_to_can[3];
				
			}

			
			if ((HVreset&0x04)==4 )
			{
				if (HVmotor3.fault!=0)
				{
					msgbfd2.speed2 = 0;
				}else{
					msgbfd2.speed2 = motors6dof.motor_to_can[4];
					HVreset = HVreset&0x03;
				}
			}else{
				msgbfd2.speed2 = motors6dof.motor_to_can[4];
			}
			
			msgbfd2.speed3 = 0;
			esc_Bfd2[_driver_index]->broadcast(msgbfd2);//大约100HZ 在没有设置参数的情况下 发送驱动*/
			//-------------------------------------------------------------------------------------------------
			msgbfd3.index = 0;
			msgbfd3.speed1 = 0;
			msgbfd3.speed2 = 0;
			msgbfd3.speed3 = 0;
			esc_Bfd3[_driver_index]->broadcast(msgbfd3);//大约100HZ 在没有设置参数的情况下 发送驱动*/


		}

		
		uint32_t nowtime2 = AP_HAL::micros();
	    static uint32_t last_send_time2 = nowtime2;	
		
		static uint32_t lasttime_reboot = 0;
		
		if(nowtime2 -	last_send_time2 >1000000UL/200 )
		{	last_send_time2 = nowtime2;
			if(stm32_rboot==1)
			{	

				static uint8_t reboot_cnt = 0;
				if((motor_from_stm32[0]!=0 || motor_from_stm32[1]!=0 ||motor_from_stm32[8]!=0 ||motor_from_stm32[9]!=0)&&(reboot_cnt<170)){
					
		            msgarryspeed.rpm.push_back(0);
					msgarryspeed.rpm.push_back(0);
					msgarryspeed.rpm.push_back(0);
					msgarryspeed.rpm.push_back(0);
					msgarryspeed.rpm.push_back(0);
					msgarryspeed.rpm.push_back(0);
					reboot_cnt++;
					
					esc_SpeedCommands[_driver_index]->broadcast(msgarryspeed);//大约100HZ 在没有设置参数的情况下 发送驱动
				}else{
					stm32_rboot = 0;
					reboot_cnt=0;
				    
				   lasttime_reboot = AP_HAL::millis();
				}
			}
			
			uint32_t errortime_reboot = 0;
			uint32_t nowtime_restart = AP_HAL::millis();//ms
			
			errortime_reboot = nowtime_restart -  lasttime_reboot;//重启时间
			if ((errortime_reboot > reboot_time)&&(stm32_rboot==0))
			{

				int16_t pwm ;
				pwm = motors6dof.motor_to_can[0];
				msgarryspeed.rpm.push_back(pwm);
				pwm = motors6dof.motor_to_can[1];
				msgarryspeed.rpm.push_back(pwm);
				pwm = motors6dof.motor_to_can[8];
				msgarryspeed.rpm.push_back(pwm);
				pwm = motors6dof.motor_to_can[9];
				msgarryspeed.rpm.push_back(pwm);
				pwm = sub.lights;
				msgarryspeed.rpm.push_back(pwm);
				pwm = sub.usblpoweroff;
				msgarryspeed.rpm.push_back(pwm);
				
				esc_SpeedCommands[_driver_index]->broadcast(msgarryspeed);//大约100HZ 在没有设置参数的情况下 发送驱动
				


			}







			
			//+++++++++设置stm32 参数----------------------------------------
				// 上位机没有发送的时候不会发送 退出初始化之后才能写参数
				if( !sub.motors.armed()&&(((set_stm32_param.flag == 0xF0)&&(set_stm32_param.number!=16))||(set_stm32_param.flag == 0xFF)))
				{

					STM32_set_packet(msgparam);
					
					SetFloatParam[_driver_index]->broadcast(msgparam);//广播发送	
					set_stm32_param.flag = 0x00;
					
					gcs().send_text(MAV_SEVERITY_WARNING, "set %x%x%x%x.%x .",msgparam.XX1,msgparam.XX2,msgparam.XX3,msgparam.XX4,msgparam.YY);	
				}
			}
		}


}



///// SRV output /////

void AP_UAVCAN::SRV_send_actuator(void)
{
    uint8_t starting_servo = 0;
    bool repeat_send;

    WITH_SEMAPHORE(SRV_sem);

    do {
        repeat_send = false;
        uavcan::equipment::actuator::ArrayCommand msg;

        uint8_t i;
        // UAVCAN can hold maximum of 15 commands in one frame
        for (i = 0; starting_servo < UAVCAN_SRV_NUMBER && i < 15; starting_servo++) {
            uavcan::equipment::actuator::Command cmd;

            /*
             * Servo output uses a range of 1000-2000 PWM for scaling.
             * This converts output PWM from [1000:2000] range to [-1:1] range that
             * is passed to servo as unitless type via UAVCAN.
             * This approach allows for MIN/TRIM/MAX values to be used fully on
             * autopilot side and for servo it should have the setup to provide maximum
             * physically possible throws at [-1:1] limits.
             */

            if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _servo_bm)) {
                cmd.actuator_id = starting_servo + 1;

                // TODO: other types
                cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS;

                // TODO: failsafe, safety
                cmd.command_value = constrain_float(((float) _SRV_conf[starting_servo].pulse - 1000.0) / 500.0 - 1.0, -1.0, 1.0);

                msg.commands.push_back(cmd);

                i++;
            }
        }

        if (i > 0) {
            act_out_array[_driver_index]->broadcast(msg);

            if (i == 15) {
                repeat_send = true;
            }
        }
    } while (repeat_send);
}

void AP_UAVCAN::SRV_send_esc(void)
{
    static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
    uavcan::equipment::esc::RawCommand esc_msg;

    uint8_t active_esc_num = 0, max_esc_num = 0;
    uint8_t k = 0;

    WITH_SEMAPHORE(SRV_sem);

    // find out how many esc we have enabled and if they are active at all
    for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) {
        if ((((uint32_t) 1) << i) & _esc_bm) {
            max_esc_num = i + 1;
            if (_SRV_conf[i].esc_pending) {
                active_esc_num++;
            }
        }
    }

    // if at least one is active (update) we need to send to all
    if (active_esc_num > 0) {
        k = 0;

        for (uint8_t i = 0; i < max_esc_num && k < 20; i++) {
            if ((((uint32_t) 1) << i) & _esc_bm) {
                // TODO: ESC negative scaling for reverse thrust and reverse rotation
                float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[i].pulse) + 1.0) / 2.0;

                scaled = constrain_float(scaled, 0, cmd_max);

                esc_msg.cmd.push_back(static_cast<int>(scaled));
            } else {
                esc_msg.cmd.push_back(static_cast<unsigned>(0));
            }

            k++;
        }

        esc_raw[_driver_index]->broadcast(esc_msg);
    }
}

void AP_UAVCAN::SRV_push_servos()
{
    WITH_SEMAPHORE(SRV_sem);

    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        // Check if this channels has any function assigned
        if (SRV_Channels::channel_function(i)) {
            _SRV_conf[i].pulse = SRV_Channels::srv_channel(i)->get_output_pwm();
            _SRV_conf[i].esc_pending = true;
            _SRV_conf[i].servo_pending = true;
        }
    }

    _SRV_armed = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED;
}


///// LED /////

void AP_UAVCAN::led_out_send()
{
    uint64_t now = AP_HAL::micros64();

    if ((now - _led_conf.last_update) < LED_DELAY_US) {
        return;
    }

    uavcan::equipment::indication::LightsCommand msg;
    {
        WITH_SEMAPHORE(_led_out_sem);

        if (_led_conf.devices_count == 0) {
            return;
        }

        uavcan::equipment::indication::SingleLightCommand cmd;

        for (uint8_t i = 0; i < _led_conf.devices_count; i++) {
            cmd.light_id =_led_conf.devices[i].led_index;
            cmd.color.red = _led_conf.devices[i].red >> 3;
            cmd.color.green = _led_conf.devices[i].green >> 2;
            cmd.color.blue = _led_conf.devices[i].blue >> 3;

            msg.commands.push_back(cmd);
        }
    }

    rgb_led[_driver_index]->broadcast(msg);
    _led_conf.last_update = now;
}

bool AP_UAVCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue)
{
    if (_led_conf.devices_count >= AP_UAVCAN_MAX_LED_DEVICES) {
        return false;
    }

    WITH_SEMAPHORE(_led_out_sem);

    // check if a device instance exists. if so, break so the instance index is remembered
    uint8_t instance = 0;
    for (; instance < _led_conf.devices_count; instance++) {
        if (_led_conf.devices[instance].led_index == led_index) {
            break;
        }
    }

    // load into the correct instance.
    // if an existing instance was found in above for loop search,
    // then instance value is < _led_conf.devices_count.
    // otherwise a new one was just found so we increment the count.
    // Either way, the correct instance is the current value of instance
    _led_conf.devices[instance].led_index = led_index;
    _led_conf.devices[instance].red = red;
    _led_conf.devices[instance].green = green;
    _led_conf.devices[instance].blue = blue;

    if (instance == _led_conf.devices_count) {
        _led_conf.devices_count++;
    }

    return true;
}

// buzzer send
void AP_UAVCAN::buzzer_send()
{
    uavcan::equipment::indication::BeepCommand msg;
    WITH_SEMAPHORE(_buzzer.sem);
    uint8_t mask = (1U << _driver_index);
    if ((_buzzer.pending_mask & mask) == 0) {
        return;
    }
    _buzzer.pending_mask &= ~mask;
    msg.frequency = _buzzer.frequency;
    msg.duration = _buzzer.duration;
    buzzer[_driver_index]->broadcast(msg);
}

// buzzer support
void AP_UAVCAN::set_buzzer_tone(float frequency, float duration_s)
{
    WITH_SEMAPHORE(_buzzer.sem);
    _buzzer.frequency = frequency;
    _buzzer.duration = duration_s;
    _buzzer.pending_mask = 0xFF;
}

// SafetyState send
void AP_UAVCAN::safety_state_send()
{
    ardupilot::indication::SafetyState msg;
    uint32_t now = AP_HAL::millis();
    if (now - _last_safety_state_ms < 500) {
        // update at 2Hz
        return;
    }
    _last_safety_state_ms = now;
    switch (hal.util->safety_switch_state()) {
    case AP_HAL::Util::SAFETY_ARMED:
        msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_OFF;
        break;
    case AP_HAL::Util::SAFETY_DISARMED:
        msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_ON;
        break;
    default:
        // nothing to send
        return;
    }
    safety_state[_driver_index]->broadcast(msg);
}

/*
  handle Button message
 */
void AP_UAVCAN::handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb)
{
    switch (cb.msg->button) {
    case ardupilot::indication::Button::BUTTON_SAFETY: {
        AP_BoardConfig *brdconfig = AP_BoardConfig::get_singleton();
        if (brdconfig && brdconfig->safety_button_handle_pressed(cb.msg->press_time)) {
            AP_HAL::Util::safety_state state = hal.util->safety_switch_state();
            if (state == AP_HAL::Util::SAFETY_ARMED) {
                hal.rcout->force_safety_on();
            } else {
                hal.rcout->force_safety_off();
            }
        }
        break;
    }
    }
}

AP_UAVCAN *AP_UAVCAN::_singleton;

namespace AP {

AP_UAVCAN &uavcan()
{
    return *AP_UAVCAN::get_singleton();
}

};


#endif // HAL_WITH_UAVCAN