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

测流修改，增加7s，CMD接收

celiu-4.0.17-rc8
zbr 4 years ago
parent
b1271f5f15
commit
1121d72ce4
13 changed files with 138 additions and 45 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. 4
      ArduCopter/Copter.h
  2. 8
      ArduCopter/Parameters.cpp
  3. 6
      ArduCopter/version.h
  4. 100
      ArduCopter/zr_flight.cpp
  5. 2
      copy.sh
  6. 2
      just_build.sh
  7. 22
      libraries/AC_Flow_Measure/AC_Flow_Measure.cpp
  8. 12
      libraries/AC_Flow_Measure/AC_Flow_Measure.h
  9. 9
      libraries/AP_BattMonitor/AP_BattMonitor.cpp
  10. 1
      libraries/AP_BattMonitor/AP_BattMonitor.h
  11. 2
      libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
  12. 2
      libraries/AP_BattMonitor/AP_BattMonitor_Params.h
  13. 13
      libraries/GCS_MAVLink/GCS_Common.cpp

4
ArduCopter/Copter.h
Unescape View File

@ -1039,6 +1039,10 @@ public: @@ -1039,6 +1039,10 @@ public:
void zr_SlowLoop();
bool flow_start_sw;
bool flow_index_sw;
uint8_t BinarySearch2f(float a[], float value, int low, int high);
uint8_t cacl_volt_pst;
};
extern Copter copter;

8
ArduCopter/Parameters.cpp
Unescape View File

@ -1667,16 +1667,16 @@ const char* Copter::get_sysid_board_id(void) @@ -1667,16 +1667,16 @@ const char* Copter::get_sysid_board_id(void)
switch (type)
{
case 0:
snprintf(buf, sizeof(buf), "Version: zr-v4.1.0 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
snprintf(buf, sizeof(buf), "Version: zr-v4.1.1 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
break;
case 1:
snprintf(buf, sizeof(buf), "Version: zr-v4.1.0 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2);
snprintf(buf, sizeof(buf), "Version: zr-v4.1.1 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2);
break;
case 2:
snprintf(buf, sizeof(buf), "Version: zr-v4.1.0 ,ID: RF610%04d%05d",(int)nameValue1,(int)nameValue2);
snprintf(buf, sizeof(buf), "Version: zr-v4.1.1 ,ID: RF610%04d%05d",(int)nameValue1,(int)nameValue2);
break;
case 3:
snprintf(buf, sizeof(buf), "Version: zr-v4.1.0 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
snprintf(buf, sizeof(buf), "Version: zr-v4.1.1 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
break;
default:

6
ArduCopter/version.h
Unescape View File

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

100
ArduCopter/zr_flight.cpp
Unescape View File

@ -52,54 +52,92 @@ void Copter::zr_SlowLoop(){ @@ -52,54 +52,92 @@ void Copter::zr_SlowLoop(){
}
uint8_t Copter::BinarySearch2f(float a[], float value, int low, int high)
{
if (low > high)
return cacl_volt_pst;
cacl_volt_pst = (low + high) / 2;
if (fabs(a[cacl_volt_pst] - value)<0.01)
//if (abs(a[mid] - value) < 30)
return cacl_volt_pst;
else if (a[cacl_volt_pst] > value)
return BinarySearch2f(a, value, low, cacl_volt_pst - 1);
else
return BinarySearch2f(a, value, cacl_volt_pst + 1, high);
}
void Copter::zr_SuperSlowLoop(){
static bool before_fly = true;
static float batt_mah_teb[] =
{
25.20,24.90,24.84,24.76,24.69,24.64,24.60,24.57,24.54,24.52,
24.50,24.48,24.45,24.43,24.40,24.36,24.32,24.26,24.21,24.14,
24.07,24.00,23.94,23.88,23.79,23.71,23.65,23.58,23.51,23.46,
23.41,23.36,23.30,23.24,23.19,23.13,23.07,23.01,22.95,22.91,
22.86,22.81,22.76,22.70,22.65,22.59,22.54,22.48,22.43,22.37,
22.31,22.26,22.21,22.16,22.11,22.06,22.01,21.96,21.92,21.88,
21.84,21.79,21.76,21.72,21.69,21.64,21.59,21.54,21.49,21.45,
21.40,21.34,21.29,21.24,21.18,21.13,21.07,21.02,20.96,20.90,
20.85,20.80,20.75,20.69,20.63,20.56,20.47,20.29,20.16,20.04,
19.88,19.73,19.53,19.34,19.19,19.00,18.80,18.51,18.00,17.40,
16.80
16.80, 17.40, 18.00, 18.51, 18.80, 19.00, 19.19, 19.34, 19.53, 19.73,
19.88, 20.04, 20.16, 20.29, 20.47, 20.56, 20.63, 20.69, 20.75, 20.80,
20.85, 20.90, 20.96, 21.02, 21.07, 21.13, 21.18, 21.24, 21.29, 21.34,
21.40, 21.45, 21.49, 21.54, 21.59, 21.64, 21.69, 21.72, 21.76, 21.79,
21.84, 21.88, 21.92, 21.96, 22.01, 22.06, 22.11, 22.16, 22.21, 22.26,
22.31, 22.37, 22.43, 22.48, 22.54, 22.59, 22.65, 22.70, 22.76, 22.81,
22.86, 22.91, 22.95, 23.01, 23.07, 23.13, 23.19, 23.24, 23.30, 23.36,
23.41, 23.46, 23.51, 23.58, 23.65, 23.71, 23.79, 23.88, 23.94, 24.00,
24.07, 24.14, 24.21, 24.26, 24.32, 24.36, 24.40, 24.43, 24.45, 24.48,
24.50, 24.52, 24.54, 24.57, 24.60, 24.64, 24.69, 24.76, 24.84, 24.90,
25.20
};
static float batt_mah_teb_7s[] =
{
21.60, 21.80, 22.04, 22.28, 22.52, 22.74, 22.95, 23.19, 23.32, 23.52,
23.65, 23.81, 23.94, 24.02, 24.10, 24.18, 24.22, 24.25, 24.30, 24.39,
24.46, 24.52, 24.60, 24.67, 24.74, 24.80, 24.86, 24.93, 24.97, 25.03,
25.08, 25.13, 25.18, 25.23, 25.27, 25.32, 25.36, 25.41, 25.45, 25.50,
25.54, 25.58, 25.63, 25.69, 25.74, 25.79, 25.85, 25.91, 25.98, 26.05,
26.10, 26.16, 26.21, 26.28, 26.35, 26.40, 26.47, 26.54, 26.61, 26.67,
26.74, 26.79, 26.85, 26.93, 27.00, 27.07, 27.13, 27.19, 27.26, 27.32,
27.38, 27.46, 27.52, 27.59, 27.66, 27.75, 27.80, 27.87, 27.94, 28.03,
28.10, 28.16, 28.21, 28.28, 28.33, 28.38, 28.43, 28.46, 28.50, 28.53,
28.56, 28.59, 28.61, 28.64, 28.67, 28.71, 28.77, 28.86, 28.97, 29.13,
29.30
};
if(motors->armed()){
before_fly = false;
if(before_fly){
before_fly = false;
gcs().send_text(MAV_SEVERITY_INFO,"%s",get_sysid_board_id());
}
relay.on(3);
gcs().send_message(MSG_ZR_FLYING_STATUS);
// camera.set_in_arm_mode(true);
}else{
relay.off(3);
// camera.set_in_arm_mode(false);
// rc().set_enable_ch();
updownStatus =UpDown_TakeOffStart;
updownStatus =UpDown_TakeOffStart;
}
if(before_fly){
uint8_t cnt,cacl_volt_pst;
float delt_volt;
for(cnt = 0; cnt<102; cnt++ ){
delt_volt = batt_mah_teb[cnt] - battery.voltage();
// delt_volt = batt_mah_teb[cnt] - test_volt;
if(delt_volt <= 0)
cacl_volt_pst = 0;
uint8_t bat_cnt;
float now_volt = battery.voltage();
switch (battery.get_batt_type(0))
{
case 0:
/* code */
break;
case 1:
bat_cnt = BinarySearch2f(batt_mah_teb,now_volt,0,100);
battery.reset_remaining(1,bat_cnt);
// gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst);
break;
case 2:
bat_cnt = BinarySearch2f(batt_mah_teb_7s,now_volt,0,100);
battery.reset_remaining(1,bat_cnt);
// gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst);
break;
default:
break;
}
cacl_volt_pst = 100 - cnt;
battery.reset_remaining(1, cacl_volt_pst);
}
// gcs().send_message(MSG_ZR_FLYING_STATUS);
// // RC6 测流开关，RC9 计数开关
// uint16_t flow_start_sw = RC_Channels::rc_channel(CH_6)->get_radio_in();
// uint16_t flow_index_sw = RC_Channels::rc_channel(CH_9)->get_radio_in();
// uint8_t measure_time = wp_nav->get_measure_time();
// int32_t real_alt = gps.location().alt;
// flow_measure.flow_auto_measure(flow_start_sw,flow_index_sw, real_alt, measure_time);
}
#endif

2
copy.sh
Unescape View File

@ -1,2 +0,0 @@ @@ -1,2 +0,0 @@
sudo cp ./build/Pixhawk4/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/zr-flow-v2-test.px4

2
just_build.sh
Unescape View File

@ -1,2 +1,4 @@ @@ -1,2 +1,4 @@
./waf configure --board Pixhawk4
./waf copter
cp ./build/Pixhawk4/bin/arducopter.apj /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/zr-flow-v2-rc3.px4

22
libraries/AC_Flow_Measure/AC_Flow_Measure.cpp
Unescape View File

@ -223,6 +223,7 @@ bool Flow_Measure::parse(uint8_t temp) @@ -223,6 +223,7 @@ bool Flow_Measure::parse(uint8_t temp)
flow_value.c[2] = data_to_mp.radar_value[23];
flow_value.c[3] = data_to_mp.radar_value[24];
data_to_mp.flow_velocity = (int32_t)(flow_value.f*100);
if(is_data_flag)
data_to_mp.cmd = 2; // ready to send
else
@ -263,12 +264,12 @@ void Flow_Measure::mav_send_flow_measure(mavlink_channel_t chan) @@ -263,12 +264,12 @@ void Flow_Measure::mav_send_flow_measure(mavlink_channel_t chan)
*
* @param msg
*/
void Flow_Measure::mav_handle_flow_measure_data(mavlink_message_t *msg)
void Flow_Measure::mav_handle_flow_measure_data(const mavlink_message_t &msg)
{
static uint8_t last_cmd = 3;
// decode message
mavlink_flow_measure_t packet;
mavlink_msg_flow_measure_decode(msg, &packet);
mavlink_msg_flow_measure_decode(&msg, &packet);
// memset(data_to_mp.raw_array,0,22);
// memcpy(data_to_mp.raw_array, packet.from_mp_array, 22);
data_to_mp.cmd = packet.cmd;
@ -427,7 +428,7 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin @@ -427,7 +428,7 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
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 ");
// gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]Send START_MEASURE command ");
}
// else if(index_sw > 1400)
// {
@ -471,7 +472,7 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin @@ -471,7 +472,7 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
set_is_data_flag(true);
send_cmd(GET_DATA);
repeat = 0;
gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]Send GET_DATA command ");
// gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]Send GET_DATA command ");
}
break;
}
@ -577,3 +578,16 @@ void Flow_Measure::update(void) @@ -577,3 +578,16 @@ void Flow_Measure::update(void)
#endif
}
// singleton instance
Flow_Measure *Flow_Measure::_singleton;
namespace AP {
Flow_Measure *flowmeasure()
{
return Flow_Measure::get_singleton();
}
}

12
libraries/AC_Flow_Measure/AC_Flow_Measure.h
Unescape View File

@ -78,13 +78,16 @@ class Flow_Measure @@ -78,13 +78,16 @@ class Flow_Measure
public:
Flow_Measure();
// get singleton instance
static Flow_Measure *get_singleton() { return _singleton; }
bool init(const AP_SerialManager& serial_manager);
bool read(void);
void update(void);
void mav_send_flow_measure(mavlink_channel_t chan);
void mav_handle_flow_measure_data(mavlink_message_t *msg);
void mav_handle_flow_measure_data(const mavlink_message_t &msg);
void MBCRC16( uint8_t c);
uint16_t MBCRC16_u( uint8_t * pucFrame, uint16_t usLen );
void crc_reset(void);
@ -113,6 +116,8 @@ public: @@ -113,6 +116,8 @@ public:
void set_flow_date_flag(bool flag){get_flow_data = flag;};
private:
static Flow_Measure *_singleton;
bool parse(uint8_t temp);
AP_HAL::UARTDriver *_uart;
uint8_t ucCRCHi;
@ -198,4 +203,9 @@ private: @@ -198,4 +203,9 @@ private:
};
namespace AP {
Flow_Measure *flowmeasure();
};
#endif /* __AC_FLOW_MEASURE_H__ */

9
libraries/AP_BattMonitor/AP_BattMonitor.cpp
Unescape View File

@ -499,6 +499,15 @@ void AP_BattMonitor::request_history(void) @@ -499,6 +499,15 @@ void AP_BattMonitor::request_history(void)
}
}
uint8_t AP_BattMonitor::get_batt_type(uint8_t i)
{
if (drivers[i] != nullptr && _params[i].type() == AP_BattMonitor_Params::BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT) {
return _params[i].batt_type();
}
return 0;
}
/*
reset battery remaining percentage for batteries that integrate to

1
libraries/AP_BattMonitor/AP_BattMonitor.h
Unescape View File

@ -166,6 +166,7 @@ public: @@ -166,6 +166,7 @@ public:
void request_info(void) ; //battgo
void request_history(void) ; //battgo
uint8_t get_batt_type(uint8_t i);
protected:

2
libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
Unescape View File

@ -171,6 +171,8 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { @@ -171,6 +171,8 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
AP_GROUPINFO("DROPOUT_V", 20, AP_BattMonitor_Params, _arming_dropout_voltage, 0),
AP_GROUPINFO("CELL_TOTAL", 21, AP_BattMonitor_Params, _batt_cells_amount, 0),
AP_GROUPINFO("BAT_TYPE", 22, AP_BattMonitor_Params, _batt_type, 1),
AP_GROUPEND

2
libraries/AP_BattMonitor/AP_BattMonitor_Params.h
Unescape View File

@ -36,6 +36,7 @@ public: @@ -36,6 +36,7 @@ public:
BattMonitor_Type type(void) const { return (enum BattMonitor_Type)_type.get(); }
BattMonitor_LowVoltage_Source failsafe_voltage_source(void) { return (enum BattMonitor_LowVoltage_Source)_failsafe_voltage_source.get(); }
int8_t batt_type( ) const { return _batt_type;} ;
AP_Int8 _type; /// 0=disabled, 3=voltage only, 4=voltage and current
AP_Int8 _volt_pin; /// board pin used to measure battery voltage
@ -58,4 +59,5 @@ public: @@ -58,4 +59,5 @@ public:
AP_Float _arming_minimum_voltage; /// voltage level required to arm
AP_Float _arming_dropout_voltage; /// voltage level required to arm
AP_Int8 _batt_cells_amount; /// voltage level required to arm
AP_Int8 _batt_type; /// voltage level required to arm
};

13
libraries/GCS_MAVLink/GCS_Common.cpp
Unescape View File

@ -41,6 +41,9 @@ @@ -41,6 +41,9 @@
#include<AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h>
#include <stdio.h>
#include <AC_Flow_Measure/AC_Flow_Measure.h>
#if HAL_RCINPUT_WITH_AP_RADIO
#include <AP_Radio/AP_Radio.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
@ -3154,6 +3157,16 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) @@ -3154,6 +3157,16 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
handle_param_value(msg);
break;
case MAVLINK_MSG_ID_FLOW_MEASURE: //@Brown , ID 19
{
Flow_Measure *flowmeasure = AP::flowmeasure();
if (flowmeasure == nullptr) {
return;
}
flowmeasure->mav_handle_flow_measure_data(msg);
break;
}
case MAVLINK_MSG_ID_SERIAL_CONTROL:
handle_serial_control(msg);
break;

Write Preview
Loading…
Cancel
Save