/*
* 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 .
*
* Author: Eugene Shamaev, Siddharth Bharat Purohit
*/
#include
#include
#if HAL_WITH_UAVCAN
#include "AP_UAVCAN.h"
#include
#include //电机类
#include "../../ArduSub/Sub.h"
#include
#include
#include
#include
#include
#include
//#include
#include
#include
#include
#include
//hight volt HighVoltThrusterSetRegs.hpp
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
//#include
//#include
//#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#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* HVoltThtSetReg[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher* esc_Bfd1[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher* esc_Bfd2[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher* esc_Bfd3[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher* clearFault[MAX_NUMBER_OF_CAN_DRIVERS];
//static uavcan::Publisher* esc_MotorCommands[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher* SetFloatParam[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher* esc_SpeedCommands[MAX_NUMBER_OF_CAN_DRIVERS];
//自定义stm32 参数设置协议--------
// publisher interfaces
static uavcan::Publisher* act_out_array[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher* esc_raw[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher* rgb_led[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher* buzzer[MAX_NUMBER_OF_CAN_DRIVERS];
static uavcan::Publisher* safety_state[MAX_NUMBER_OF_CAN_DRIVERS];
// subscribers
// handler SafteyButton
UC_REGISTRY_BINDER(ButtonCb, ardupilot::indication::Button);
static uavcan::Subscriber *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::can().get_driver(driver_index));
}
//self define rec fun-------------------------------start---------------
static void motor_res_st_cb(const uavcan::ReceivedDataStructure& 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& msg ){
motor_res_st_cb(msg,0);
}
static void motor_res_st_cb1(const uavcan::ReceivedDataStructure& msg ){
motor_res_st_cb(msg,1);
}
static void (*motor_res_st_cb_arr[2])(const uavcan::ReceivedDataStructure& 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& 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& msg ){
motor_par1_cb(msg,0);
}
static void motor_par1_cb1(const uavcan::ReceivedDataStructure& msg ){
motor_par1_cb(msg,1);
}
static void (*motor_par1_cb_arr[2])(const uavcan::ReceivedDataStructure& msg )
={motor_par1_cb0,motor_par1_cb1};
//高压电调1参数返回
static void thruster1_1GetRegs_cb(const uavcan::ReceivedDataStructure& 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& msg ){
thruster1_1GetRegs_cb(msg,0);
}
static void Thruster1_1GetRegs_cb1(const uavcan::ReceivedDataStructure& msg ){
thruster1_1GetRegs_cb(msg,1);
}
static void (*Thruster1_1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure& msg )
={Thruster1_1GetRegs_cb0,Thruster1_1GetRegs_cb1};
//福建
static void thruster1GetRegs_cb(const uavcan::ReceivedDataStructure& 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& msg ){
thruster1GetRegs_cb(msg,0);
}
static void Thruster1GetRegs_cb1(const uavcan::ReceivedDataStructure& msg ){
thruster1GetRegs_cb(msg,1);
}
static void (*Thruster1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure& msg )
={Thruster1GetRegs_cb0,Thruster1GetRegs_cb1};
//高压电调2参数返回
static void thruster2_1GetRegs_cb(const uavcan::ReceivedDataStructure& 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& msg ){
thruster2_1GetRegs_cb(msg,0);
}
static void Thruster2_1GetRegs_cb1(const uavcan::ReceivedDataStructure& msg ){
thruster2_1GetRegs_cb(msg,1);
}
static void (*Thruster2_1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure& msg )
={Thruster2_1GetRegs_cb0,Thruster2_1GetRegs_cb1};
//福建
static void thruster2GetRegs_cb(const uavcan::ReceivedDataStructure& 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& msg ){
thruster2GetRegs_cb(msg,0);
}
static void Thruster2GetRegs_cb1(const uavcan::ReceivedDataStructure& msg ){
thruster2GetRegs_cb(msg,1);
}
static void (*Thruster2GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure& msg )
={Thruster2GetRegs_cb0,Thruster2GetRegs_cb1};
//高压电调3参数返回
static void thruster3_1GetRegs_cb(const uavcan::ReceivedDataStructure& 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& msg ){
thruster3_1GetRegs_cb(msg,0);
}
static void Thruster3_1GetRegs_cb1(const uavcan::ReceivedDataStructure& msg ){
thruster3_1GetRegs_cb(msg,1);
}
static void (*Thruster3_1GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure& msg )
={Thruster3_1GetRegs_cb0,Thruster3_1GetRegs_cb1};
//福建
static void thruster3GetRegs_cb(const uavcan::ReceivedDataStructure& 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& msg ){
thruster3GetRegs_cb(msg,0);
}
static void Thruster3GetRegs_cb1(const uavcan::ReceivedDataStructure& msg ){
thruster3GetRegs_cb(msg,1);
}
static void (*Thruster3GetRegs_cb_arr[2])(const uavcan::ReceivedDataStructure& 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& msg ,uint8_t mgr){
static void Thruster1timeback_cb(const uavcan::ReceivedDataStructure& 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& msg ){
static void Thruster1timeback_cb0(const uavcan::ReceivedDataStructure& msg ){
Thruster1timeback_cb(msg,0);
}
//static void Thruster1timeback_cb1(const uavcan::ReceivedDataStructure& msg ){
static void Thruster1timeback_cb1(const uavcan::ReceivedDataStructure& msg ){
Thruster1timeback_cb(msg,1);
}
//static void (*Thruster1timeback_cb_arr[2])(const uavcan::ReceivedDataStructure& msg )
static void (*Thruster1timeback_cb_arr[2])(const uavcan::ReceivedDataStructure& msg )
={Thruster1timeback_cb0,Thruster1timeback_cb1};
//高压电调2定时返回
//static void Thruster2timeback_cb(const uavcan::ReceivedDataStructure& msg ,uint8_t mgr){
static void Thruster2timeback_cb(const uavcan::ReceivedDataStructure& 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& msg ){
static void Thruster2timeback_cb0(const uavcan::ReceivedDataStructure& msg ){
Thruster2timeback_cb(msg,0);
}
//static void Thruster2timeback_cb1(const uavcan::ReceivedDataStructure& msg ){
static void Thruster2timeback_cb1(const uavcan::ReceivedDataStructure& msg ){
Thruster2timeback_cb(msg,1);
}
//static void (*Thruster2timeback_cb_arr[2])(const uavcan::ReceivedDataStructure& msg )
static void (*Thruster2timeback_cb_arr[2])(const uavcan::ReceivedDataStructure& msg )
={Thruster2timeback_cb0,Thruster2timeback_cb1};
//高压电调2定时返回
//static void Thruster3timeback_cb(const uavcan::ReceivedDataStructure& msg ,uint8_t mgr){
static void Thruster3timeback_cb(const uavcan::ReceivedDataStructure& 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& msg ){
static void Thruster3timeback_cb0(const uavcan::ReceivedDataStructure& msg ){
Thruster3timeback_cb(msg,0);
}
//static void Thruster3timeback_cb1(const uavcan::ReceivedDataStructure& msg ){
static void Thruster3timeback_cb1(const uavcan::ReceivedDataStructure& msg ){
Thruster3timeback_cb(msg,1);
}
//static void (*Thruster3timeback_cb_arr[2])(const uavcan::ReceivedDataStructure& msg )
static void (*Thruster3timeback_cb_arr[2])(const uavcan::ReceivedDataStructure& 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* motor_res_st;
motor_res_st = new uavcan::Subscriber(*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* motor_par1_st;
motor_par1_st = new uavcan::Subscriber(*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* Thruster1_1GetRegs;
Thruster1_1GetRegs = new uavcan::Subscriber(*node);
const int Tht1_1GetReg = Thruster1_1GetRegs->start(Thruster1_1GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
if (Tht1_1GetReg<0)
{
return;
}
//高压电调2 参数返回
uavcan::Subscriber* Thruster2_1GetRegs;
Thruster2_1GetRegs = new uavcan::Subscriber(*node);
const int Tht2_1GetReg = Thruster2_1GetRegs->start(Thruster2_1GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
if (Tht2_1GetReg<0)
{
return;
}
//高压电调3 参数返回
uavcan::Subscriber* Thruster3_1GetRegs;
Thruster3_1GetRegs = new uavcan::Subscriber(*node);
const int Tht3_1GetReg = Thruster3_1GetRegs->start(Thruster3_1GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
if (Tht3_1GetReg<0)
{
return;
}//以下是福建的--------------------------
//高压电调1 参数返回
uavcan::Subscriber* Thruster1GetRegs;
Thruster1GetRegs = new uavcan::Subscriber(*node);
const int Tht1GetReg = Thruster1GetRegs->start(Thruster1GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
if (Tht1GetReg<0)
{
return;
}
//高压电调2 参数返回
uavcan::Subscriber* Thruster2GetRegs;
Thruster2GetRegs = new uavcan::Subscriber(*node);
const int Tht2GetReg = Thruster2GetRegs->start(Thruster2GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
if (Tht2GetReg<0)
{
return;
}
//高压电调3 参数返回
uavcan::Subscriber* Thruster3GetRegs;
Thruster3GetRegs = new uavcan::Subscriber(*node);
const int Tht3GetReg = Thruster3GetRegs->start(Thruster3GetRegs_cb_arr[_driver_index]);//定义回调函数用于接收
if (Tht3GetReg<0)
{
return;
}
//高压电调1 定时参数返回
uavcan::Subscriber* Thruster1timeback;
Thruster1timeback = new uavcan::Subscriber(*node);
const int Tht1timecb = Thruster1timeback->start(Thruster1timeback_cb_arr[_driver_index]);//定义回调函数用于接收
if (Tht1timecb<0)
{
return;
}
//高压电调2 定时参数返回
uavcan::Subscriber* Thruster2timeback;
Thruster2timeback = new uavcan::Subscriber(*node);
const int Tht2timecb = Thruster2timeback->start(Thruster2timeback_cb_arr[_driver_index]);//定义回调函数用于接收
if (Tht2timecb<0)
{
return;
}
//高压电调3 定时参数返回
uavcan::Subscriber* Thruster3timeback;
Thruster3timeback = new uavcan::Subscriber(*node);
const int Tht3timecb = Thruster3timeback->start(Thruster3timeback_cb_arr[_driver_index]);//定义回调函数用于接收
if (Tht3timecb<0)
{
return;
}
//创建发送设备类型
esc_SpeedCommands[driver_index] = new uavcan::Publisher(*_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(*_node);
clearFault[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
clearFault[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
//高压电调参数设置
HVoltThtSetReg[driver_index] = new uavcan::Publisher(*_node);
HVoltThtSetReg[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
HVoltThtSetReg[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
//创建发送设备类型
esc_Bfd1[driver_index] = new uavcan::Publisher(*_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(*_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(*_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(*_node);
// esc_MotorCommands[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
// esc_MotorCommands[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
//创建参数设置类型
SetFloatParam[driver_index] = new uavcan::Publisher(*_node);
SetFloatParam[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
SetFloatParam[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
//-----------------------------------
act_out_array[driver_index] = new uavcan::Publisher(*_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(*_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(*_node);
rgb_led[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
rgb_led[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
buzzer[driver_index] = new uavcan::Publisher(*_node);
buzzer[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
buzzer[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
safety_state[driver_index] = new uavcan::Publisher(*_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(*_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(scaled));
} else {
esc_msg.cmd.push_back(static_cast(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