12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412 |
- /*
- * 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/HighVoltThrusterSetRegs.hpp>
- #include <uavcan/equipment/esc/HighVoltThruster1GetRegs.hpp>
- #include <uavcan/equipment/esc/HighVoltThruster2GetRegs.hpp>
- #include <uavcan/equipment/esc/HighVoltThruster3GetRegs.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::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;//推进器缠绕等级
- 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 current_trust=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;
- }
-
- motor_stall_state.all = 0;
- propeller_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;
- propeller_block.bit.motor0 = (msg.rpm0 & 0xF000)>>12;
- motors_receive[1] = msg.rpm1 & 0x0FFF;
- propeller_block.bit.motor1 = (msg.rpm1 & 0xF000)>>12;
- motors_receive[2] = msg.rpm2 & 0x0FFF;
- propeller_block.bit.motor2 = (msg.rpm2 & 0xF000)>>12;
-
- // gcs().send_text(MAV_SEVERITY_WARNING, "motor_index = 0 %d %d %d.",(int)motors_receive[0],(int)motors_receive[1],motors_receive[2]);
-
- }
- /*else if(msg.motor_index == 3){
- //推进器3,4,5
- motors_receive[3] = msg.rpm0 & 0x0FFF;
- propeller_block.bit.motor3 = (msg.rpm0 & 0xF000)>>12;
- motors_receive[4] = msg.rpm1 & 0x0FFF;
- propeller_block.bit.motor4 = (msg.rpm1 & 0xF000)>>12;
- motors_receive[5] = msg.rpm2 & 0x0FFF;
- propeller_block.bit.motor5 = (msg.rpm2 & 0xF000)>>12;
- static int16_t c2 = 0;
- c2++;
- if(c2>100){
- c2 =0 ;
- //gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 3 %d %d %d.",(int)motors_receive[3],(int)motors_receive[4],motors_receive[5]);
-
- }
- }*/
- else if(msg.motor_index == 6){
- ////推进器6,7,履带1
- motors_receive[6] = msg.rpm0 & 0x0FFF;
- propeller_block.bit.motor6 = (msg.rpm0 & 0xF000)>>12;
- motors_receive[7] = msg.rpm1 & 0x0FFF;
- propeller_block.bit.motor7 = (msg.rpm1 & 0xF000)>>12;
- motors_receive[8] = msg.rpm2 & 0x0FFF;
-
- static int16_t c3 = 0;
- c3++;
- if(c3>100){
- c3 =0 ;
- //gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 6 %d %d %d",(int)motors_receive[6],(int)motors_receive[7],motors_receive[8]);
-
- }
- }
- else if(msg.motor_index == 9){
- ////履带2,毛刷1,毛刷2
- motors_receive[9] = msg.rpm0 & 0x0FFF;
-
- motors_receive[10] = msg.rpm1 & 0x0FFF;
-
- motors_receive[11] = msg.rpm2 & 0x0FFF;
-
- static int16_t c4 = 0;
- c4++;
- if(c4>100){
- c4 =0 ;
- //gcs().send_text(MAV_SEVERITY_WARNING, "motor_index == 9 %d %d %d.",(int)motors_receive[9],(int)motors_receive[10],motors_receive[11]);
-
- }
- }
- 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;
- current_trust = msg.rpm2;
- static int16_t c6 = 0;
- c6++;
- if(c6>100){
- c6 =0 ;
- //gcs().send_text(MAV_SEVERITY_WARNING, "15-- %d .",(int)current_trust);
-
- }
- }
- }
- 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 thruster1GetRegs_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 Thruster1GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg ){
- thruster1GetRegs_cb(msg,0);
- }
- static void Thruster1GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg ){
- thruster1GetRegs_cb(msg,1);
- }
- static void (*Thruster1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster1GetRegs>& msg )
- ={Thruster1GetRegs_cb0,Thruster1GetRegs_cb1};
- //高压电调2参数返回
- static void thruster2GetRegs_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 Thruster2GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg ){
- thruster2GetRegs_cb(msg,0);
- }
- static void Thruster2GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg ){
- thruster2GetRegs_cb(msg,1);
- }
- static void (*Thruster2GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster2GetRegs>& msg )
- ={Thruster2GetRegs_cb0,Thruster2GetRegs_cb1};
- //高压电调3参数返回
- static void thruster3GetRegs_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 Thruster3GetRegs_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& msg ){
- thruster3GetRegs_cb(msg,0);
- }
- static void Thruster3GetRegs_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& msg ){
- thruster3GetRegs_cb(msg,1);
- }
- static void (*Thruster3GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::HighVoltThruster3GetRegs>& 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 int8_t per = 0;
- per++;
- if (per>100)
- {
- per = 0;
- gcs().send_text(MAV_SEVERITY_INFO, "HV3 %d %d %d %d",(int)Thruster1.voltage,(int)Thruster1.speedref,(int)Thruster1.temperature,(int)Thruster1.motortemperature);
- }
-
- //}
-
- }
- //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 int8_t per = 0;
- per++;
- if (per>100)
- {
- per = 0;
- gcs().send_text(MAV_SEVERITY_INFO, "HV4 %d %d %d %d",(int)Thruster2.voltage,(int)Thruster2.speedref,(int)Thruster2.temperature,(int)Thruster2.motortemperature);
- }
-
- //}
-
- }
- //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 int8_t per = 0;
- per++;
- if (per>100)
- {
- per = 0;
- gcs().send_text(MAV_SEVERITY_INFO, "HV5 %d %d %d %d",(int)Thruster3.voltage,(int)Thruster3.speedref,(int)Thruster3.temperature,(int)Thruster3.motortemperature);
- }
-
- //}
- }
- //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>* Thruster1GetRegs;
- Thruster1GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster1GetRegs>(*node);
- const int Tht1GetReg = Thruster1GetRegs->start(Thruster1GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
- if (Tht1GetReg<0)
- {
- return;
- }
- //高压电调2 参数返回
- uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster2GetRegs>* Thruster2GetRegs;
- Thruster2GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster2GetRegs>(*node);
- const int Tht2GetReg = Thruster2GetRegs->start(Thruster2GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
- if (Tht2GetReg<0)
- {
- return;
- }
- //高压电调3 参数返回
- uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster3GetRegs>* Thruster3GetRegs;
- Thruster3GetRegs = new uavcan::Subscriber<uavcan::equipment::esc::HighVoltThruster3GetRegs>(*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
- //高压电调参数设置
- 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;//推进器缠绕等级
- temperature_48Vpower = temperature_power;//can获取18B20温度
- board_voltage = voltage48V;//can获取48V电源
- driverleakstate = driveleak;
-
- 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*******************************************
-
- //---------重启设置---------------------------
-
-
- uint32_t nowtime_restart = AP_HAL::millis();//ms
- static uint32_t lasttime_reboot = 0;
-
- //if(!sub.motors.armed()&&(set_stm32_param.flag == 0xF0)&&(set_stm32_param.number ==12)){
- if((set_stm32_param.flag == 0xF0)&&(set_stm32_param.number ==12)){
- //消除缠绕
- if (!sub.motors.armed())
- {
- lasttime_reboot = nowtime_restart;
- }
-
- set_stm32_param.flag = 0;
-
- //高压电调消除故障
- //crc_ackfault(hvmesg,0);
- HVreset = 7;
-
- //HVoltThtSetReg[_driver_index]->broadcast(hvmesg);//消除所有高压电调的故障
- }
-
- uint32_t errortime_reboot = 0;
- errortime_reboot = nowtime_restart - lasttime_reboot;//重启时间
- //-------发送数据-------------------------------------------------
- const AP_Motors6DOF &motors6dof = AP::motors6dof();//6自由度电机计算出来的PWM
- uavcan::equipment::esc::SingleMotorCommand msg;
- //uavcan::equipment::esc::MotorCommands msgarry;
- uavcan::equipment::esc::SetFloatParam msgparam;
- uavcan::equipment::esc::SpeedCommands msgarryspeed;
- 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;
- if( (nowtime2 - last_send_time2 >1000000UL/200 )&& (errortime_reboot > reboot_time))//发送周期
- { last_send_time2 = nowtime2;
- 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 在没有设置参数的情况下 发送驱动
- static int f = 0;
- f++;
- if(f>200)
- {
-
- //gcs().send_text(MAV_SEVERITY_INFO, " HVreset %d,%x,%x,%x,%x ",(int)test,HVreset,(int)HVmotor1.fault,(int)HVmotor2.fault,(int)HVmotor3.fault);
- //gcs().send_text(MAV_SEVERITY_INFO, " hvmot %d %d %d,",(int)motors6dof.motor_to_can[2],(int)motors6dof.motor_to_can[3],(int)motors6dof.motor_to_can[4]);
- f=0;
- }
-
- //+++++++++设置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
|