zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

测流实测可用，增加角度解析

celiu-4.0.17-rc8
zbr 4 years ago
parent
68bc698a04
commit
07f1eeb503
3 changed files with 101 additions and 47 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      ArduCopter/version.h
  2. 118
      libraries/AC_Flow_Measure/AC_Flow_Measure.cpp
  3. 12
      libraries/AC_Flow_Measure/AC_Flow_Measure.h

8
ArduCopter/version.h
Unescape View File

@ -6,12 +6,12 @@ @@ -6,12 +6,12 @@
#include "ap_version.h"
#define THISFIRMWARE "ZRUAV v4.1.6" //"ArduCopter V4.0.0"
#define THISFIRMWARE "ZRUAV v4.2.8" //"ArduCopter V4.0.0"
// the following line is parsed by the autotest scripts
#define FIRMWARE_VERSION 4,1,6,FIRMWARE_VERSION_TYPE_OFFICIAL
#define FIRMWARE_VERSION 4,2,8,FIRMWARE_VERSION_TYPE_OFFICIAL
#define FW_MAJOR 4
#define FW_MINOR 1
#define FW_PATCH 6
#define FW_MINOR 2
#define FW_PATCH 8
#define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL

118
libraries/AC_Flow_Measure/AC_Flow_Measure.cpp
Unescape View File

@ -27,7 +27,6 @@ @@ -27,7 +27,6 @@
extern const AP_HAL::HAL& hal;
#define FLOW_HEAD 0X34
// static const uint8_t start_measure_cmd[11] = {0x34, 0x10, 0x00, 0x67, 0x00, 0x01, 0x02, 0xAA, 0xBB, 0xFB, 0xC5};
// // static const uint8_t time_measure_cmd[11] = {0x34, 0x10, 0x00, 0x66, 0x00, 0x01, 0x02, 0x00, 0x64, 0xC5, 0x2C};
// static uint8_t time_measure_cmd[11] = {0x34, 0x10, 0x00, 0x66, 0x00, 0x01, 0x02, 0x00, 0x64, 0xC5, 0x2C};
@ -36,6 +35,7 @@ extern const AP_HAL::HAL& hal; @@ -36,6 +35,7 @@ extern const AP_HAL::HAL& hal;
static const uint8_t start_measure_cmd[11] = {0x00, 0x10, 0x00, 0x67, 0x00, 0x01, 0x02, 0xaa, 0xbb, 0x9d, 0x04};
// static const uint8_t time_measure_cmd[11] = {0x34, 0x10, 0x00, 0x66, 0x00, 0x01, 0x02, 0x00, 0x64, 0xC5, 0x2C};
static uint8_t time_measure_cmd[11] = {0x00, 0x10, 0x00, 0x66, 0x00, 0x01, 0x02, 0x00, 0x64, 0xa3, 0xed};
static uint8_t get_angle_cmd[8] = {0x00, 0x03, 0x00, 0x09, 0x00, 0x02, 0xe5, 0xdd};
static const uint8_t get_data_cmd[8] = {0x00, 0x03, 0x00, 0x06, 0x00, 0x0b, 0xe5, 0xdd};
static const uint8_t get_data_cmd_single[8] = {0x00, 0x03, 0x00, 0x0F, 0x00, 0x02, 0xF5, 0xD9};
@ -268,7 +268,7 @@ bool Flow_Measure::parse(uint8_t temp) @@ -268,7 +268,7 @@ bool Flow_Measure::parse(uint8_t temp)
Debug("read complete");
memcpy( data_to_mp.radar_value, radar_data.data,read_len+1);
if(is_data_flag)
if(is_data_flag && single_measure_mode == 0)
data_to_mp.cmd = 2; // ready to send
else
data_to_mp.cmd = 1; // ready to send
@ -319,7 +319,7 @@ void Flow_Measure::mav_handle_flow_measure_data(const mavlink_message_t &msg) @@ -319,7 +319,7 @@ void Flow_Measure::mav_handle_flow_measure_data(const mavlink_message_t &msg)
// memcpy(data_to_mp.raw_array, packet.from_mp_array, 22);
data_to_mp.cmd = packet.cmd;
// gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]mav receive: %d,msg:%d",data_to_mp.cmd,msg.payload64[8]);
send_to_mavlink_cnt = 5; // 发送5次Mavlink消息
send_to_mavlink_cnt = 6; // 发送5次Mavlink消息
send_cmd_to_radar( packet.from_mp_array,sizeof(packet.from_mp_array));
// gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]arr:%02x %02x %02x %02x ",packet.from_mp_array[0],packet.from_mp_array[1],packet.from_mp_array[2],packet.from_mp_array[3]);
}
@ -384,7 +384,17 @@ void Flow_Measure::send_cmd(cmd_type_t type) @@ -384,7 +384,17 @@ void Flow_Measure::send_cmd(cmd_type_t type)
break;
case GET_DATA_SINGLE:
send_cmd_to_radar(get_data_cmd_single,8);
break;
case GET_ANGLE:
crc_reset();
for(int i = 0; i < 6; i++)
{
MBCRC16(get_angle_cmd[i]);
}
get_angle_cmd[6] = ucCRCLo;
get_angle_cmd[7] = ucCRCHi;
send_cmd_to_radar(get_angle_cmd,8);
break;
}
// data_to_mp.cmd = 0; // disable send
@ -494,24 +504,31 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin @@ -494,24 +504,31 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
set_is_data_flag(false);
send_cmd(START_MEASURE);
repeat = 0;
// gcs().send_text(MAV_SEVERITY_NOTICE, "[INFO]设置测流历时: %3ds",measure_time);
// gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]Send START_MEASURE command ");
}
// else if(index_sw > 1400)
// {
// flow_update_state = 2;
// gcs().send_text(MAV_SEVERITY_INFO, "[INFO]Skip this WP");
// // return true;
// }
break;
case 2:
// send_cmd(START_MEASURE);
if(get_radar_data())
{
flow_update_state = 3;
data_package_angle();
disable_data_ready();
repeat = 0;
}else if(repeat++ > 2){
set_is_data_flag(false);
send_cmd(GET_ANGLE);
repeat = 0;
}
break;
case 3:
if (((AP_HAL::millis() - mark_time) / 1000) >= measure_time){
flow_update_state = 3;
flow_update_state = 4;
}
break;
case 3:
case 4:
// send_cmd(GET_DATA);
if(get_radar_data())
{
@ -521,23 +538,46 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin @@ -521,23 +538,46 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
}
flow_update_state = 1;
get_gps_alt(alt + delt_alt);
if(single_measure_mode == 0){
data_package();
}
else{
data_package_single();
}
// gcs_send_message(MSG_FLOW_MEASURE);
disable_data_ready();
// get_flow_data = true; // 获取到流速，航点到达判断成功
set_flow_date_flag(true);
if(manual_cnt == 1)
gcs().send_text(MAV_SEVERITY_CRITICAL, "[格式] 序号, 历时, 海拔, 空高, 流速");
// gcs().send_text(MAV_SEVERITY_NOTICE, "[DATA]i:%2d,time:%3d,alt_a:%4d,alt_r:%4d,measure:%4d",get_wp_index(),wp_nav->get_measure_time(),loc.alt,get_radar_alt(),get_flow_value());
gcs().send_text(MAV_SEVERITY_CRITICAL, "[数据] %2d, %3ds, %4.2fm, %2.2fm, %4d",get_wp_index(),measure_time,alt/100.0,radar_alt/1000.0,data_to_mp.flow_velocity);
set_is_data_flag(true);
if(single_measure_mode == 0){
if(manual_cnt == 1)
gcs().send_text(MAV_SEVERITY_CRITICAL, "[格式] 序号, 历时, 角度, 海拔, 空高, 流速");
gcs().send_text(MAV_SEVERITY_CRITICAL, "[数据] %2d, %2ds, %2.1f, %4.2fm, %2.2fm, %2.2f",get_wp_index(),measure_time,pitch.d/10.0,alt/100.0,radar_alt/1000.0,data_to_mp.flow_velocity/100.0);
}else{
if(manual_cnt == 1)
gcs().send_text(MAV_SEVERITY_CRITICAL, "[格式] 序号, 历时, 角度, 流速, 数组");
gcs().send_text(MAV_SEVERITY_CRITICAL, "%2d, %2ds, %2.1f, %2.2f, 0x%02x%02x%02x%02x",get_wp_index(),measure_time,pitch.d/10.0,data_to_mp.flow_velocity/100.0, \
data_to_mp.radar_value[3],data_to_mp.radar_value[4],data_to_mp.radar_value[5],data_to_mp.radar_value[6]);
}
repeat = 0;
send_to_mavlink_cnt = 5; // 发送5次Mavlink消息
send_to_mavlink_cnt = 6; // 发送5次Mavlink消息
}else if(repeat++ > 4){
set_is_data_flag(true);
if(single_measure_mode == 0){
send_cmd(GET_DATA);
}else{
send_cmd(GET_DATA_SINGLE);
}
repeat = 0;
// gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]Send GET_DATA command ");
}
@ -559,33 +599,29 @@ void Flow_Measure::data_package_single(){ @@ -559,33 +599,29 @@ void Flow_Measure::data_package_single(){
pack_data.value.head = FLOW_HEAD;
pack_data.value.flag = 0x03;
pack_data.value.length = 0x1C;
// if(data_to_mp.index == 0)
// data_to_mp.index = 1;
// pack_data.value.index = data_to_mp.index;
pack_data.value.index = (data_to_mp.index) | ((data_to_mp.index+1) << 8);
pack_data.value.alt = data_to_mp.hh_elevation;
ch2float flow_value;
flow_value.c[0] = data_to_mp.radar_value[4];
flow_value.c[1] = data_to_mp.radar_value[5];
flow_value.c[2] = data_to_mp.radar_value[6];
flow_value.c[3] = data_to_mp.radar_value[7];
flow_value.c[0] = data_to_mp.radar_value[3];
flow_value.c[1] = data_to_mp.radar_value[4];
flow_value.c[2] = data_to_mp.radar_value[5];
flow_value.c[3] = data_to_mp.radar_value[6];
data_to_mp.flow_velocity = (int32_t)(flow_value.f*100);
my_endianswap(pack_data.data,3,2);
my_endianswap(pack_data.data,5,4);
memcpy( pack_data.value.radar_array, radar_data.value.radar_arr,22);
radar_alt = radar_data.value.radar_arr[0]<<8 | radar_data.value.radar_arr[1];
crc_reset();
for(int i = 0; i < 31; i++)
{
MBCRC16(pack_data.data[i]);
}
pack_data.value.crc_l = ucCRCLo;
pack_data.value.crc_h = ucCRCHi;
memcpy( data_to_mp.raw_array, pack_data.data,44); // 拷贝打包的数据到raw_array
void Flow_Measure::data_package_angle(){
roll.c[1] = data_to_mp.radar_value[3];
roll.c[0] = data_to_mp.radar_value[4];
pitch.c[1] = data_to_mp.radar_value[5];
pitch.c[0] = data_to_mp.radar_value[6];
// gcs().send_text(MAV_SEVERITY_CRITICAL, "[数据] Roll:%d, Pitch:%d,RAW:%02x% 02x %02x% 02x",roll.d,pitch.d,data_to_mp.radar_value[3],data_to_mp.radar_value[4],data_to_mp.radar_value[5],data_to_mp.radar_value[6]);
}
void Flow_Measure::data_package()
{
#if USE_SITL
@ -689,10 +725,18 @@ void Flow_Measure::update(void) @@ -689,10 +725,18 @@ void Flow_Measure::update(void)
#else
read();
static uint32_t last_1s;
uint32_t tnow = AP_HAL::millis();
if (tnow - last_1s > 1000) { // 1s 发送一次测流MAVLINK
last_1s = tnow;
if(send_to_mavlink_cnt>0){
send_to_mavlink_cnt -=1 ;
if(send_to_mavlink_cnt < 3)
data_to_mp.cmd = 0; // disable send
gcs().send_message(MSG_FLOW_MEASURE);
}
}
#endif
}

12
libraries/AC_Flow_Measure/AC_Flow_Measure.h
Unescape View File

@ -71,7 +71,8 @@ typedef enum { @@ -71,7 +71,8 @@ typedef enum {
SET_TIME,
START_MEASURE,
GET_DATA,
GET_DATA_SINGLE
GET_DATA_SINGLE,
GET_ANGLE
} cmd_type_t;
class Flow_Measure
@ -118,6 +119,8 @@ public: @@ -118,6 +119,8 @@ public:
void read_uavcan_data(uint8_t *data,uint8_t len);
void data_package_single();
void data_package_angle();
private:
static Flow_Measure *_singleton;
@ -153,6 +156,10 @@ private: @@ -153,6 +156,10 @@ private:
float f;
unsigned char c[4];
}ch2float;
typedef union{
int16_t d;
unsigned char c[2];
}ch2int16;
union PACKED radar_data_u { // 联合体
radar_data_s value;
uint8_t data[50]; // 串口接收
@ -206,6 +213,9 @@ private: @@ -206,6 +213,9 @@ private:
bool get_flow_data;
bool get_uanvcan_data;
uint8_t uavcan_data[27];
const uint8_t single_measure_mode = 1;
ch2int16 roll,pitch;
};
namespace AP {

Write Preview
Loading…
Cancel
Save