diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 930fd8a924..9fa4465a31 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -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; diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 3a95e7c548..031c840ce4 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -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: diff --git a/ArduCopter/version.h b/ArduCopter/version.h index c2a2476a61..6ca1cf7286 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -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 diff --git a/ArduCopter/zr_flight.cpp b/ArduCopter/zr_flight.cpp index 7a94fef34f..6e48c3a87e 100644 --- a/ArduCopter/zr_flight.cpp +++ b/ArduCopter/zr_flight.cpp @@ -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 \ No newline at end of file diff --git a/copy.sh b/copy.sh deleted file mode 100755 index 55c71b59df..0000000000 --- a/copy.sh +++ /dev/null @@ -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 - diff --git a/just_build.sh b/just_build.sh index 16ab51fd80..03cec48440 100755 --- a/just_build.sh +++ b/just_build.sh @@ -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 + diff --git a/libraries/AC_Flow_Measure/AC_Flow_Measure.cpp b/libraries/AC_Flow_Measure/AC_Flow_Measure.cpp index 29b6be8d92..3fb2c837a9 100755 --- a/libraries/AC_Flow_Measure/AC_Flow_Measure.cpp +++ b/libraries/AC_Flow_Measure/AC_Flow_Measure.cpp @@ -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) * * @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 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 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) #endif } + + +// singleton instance +Flow_Measure *Flow_Measure::_singleton; + +namespace AP { + + Flow_Measure *flowmeasure() + { + return Flow_Measure::get_singleton(); + } + +} diff --git a/libraries/AC_Flow_Measure/AC_Flow_Measure.h b/libraries/AC_Flow_Measure/AC_Flow_Measure.h index 7b7256fb0c..513f7783c5 100755 --- a/libraries/AC_Flow_Measure/AC_Flow_Measure.h +++ b/libraries/AC_Flow_Measure/AC_Flow_Measure.h @@ -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: 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: }; +namespace AP { + Flow_Measure *flowmeasure(); +}; + + #endif /* __AC_FLOW_MEASURE_H__ */ \ No newline at end of file diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 126ab5c6e5..0b8e4e305d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 7cee5334be..bc4fe3b3d1 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -166,6 +166,7 @@ public: void request_info(void) ; //battgo void request_history(void) ; //battgo + uint8_t get_batt_type(uint8_t i); protected: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 5ae21589b0..906e92bbd6 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 18cca08943..31488e8ea2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -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: 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 }; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 9569bbac0b..3658a07c2d 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -41,6 +41,9 @@ #include #include +#include + + #if HAL_RCINPUT_WITH_AP_RADIO #include #include @@ -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;