From badc75ee5c85211df84343a7bb4847bd46986ef0 Mon Sep 17 00:00:00 2001 From: Noppawit Lertutsahakul Date: Thu, 16 Jan 2020 14:09:45 +0700 Subject: [PATCH 01/15] AP_Arming: Fix prearm text overflow. --- libraries/AP_Arming/AP_Arming.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index b56e882de8723..3a7e7c58a0682 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -446,7 +446,7 @@ bool AP_Arming::battery_checks(bool report) for (uint8_t i = 0; i < _battery.num_instances(); i++) { if ((_min_voltage[i] > 0.0f) && (_battery.voltage(i) < _min_voltage[i])) { - check_failed(ARMING_CHECK_BATTERY, report, "PreArm: Battery %d voltage %.1f below minimum %.1f", + check_failed(ARMING_CHECK_BATTERY, report, "Battery %d voltage %.1f below minimum %.1f", i+1, (double)_battery.voltage(i), (double)_min_voltage[i]); From 2ebaebf133598685ff406d8cc9952adbdc96d5fe Mon Sep 17 00:00:00 2001 From: tony2157 Date: Fri, 19 Jun 2020 13:59:26 -0500 Subject: [PATCH 02/15] Enable another User function slot for the implementation of a custom battery management --- ArduCopter/APM_Config.h | 2 +- ArduCopter/ArduCopter.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 34ac790357c49..e5243c3eea4a9 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -106,7 +106,7 @@ // Put your custom code into the UserCode.cpp with function names matching those listed below and ensure the appropriate #define below is uncommented below #define USERHOOK_INIT userhook_init(); // for code to be run once at startup //#define USERHOOK_FASTLOOP userhook_FastLoop(); // for code to be run at 100hz -//#define USER_VPBATT_LOOP user_vpbatt_monitor(); // for code to be run at 50hz +#define USER_VPBATT_LOOP user_vpbatt_monitor(); // for code to be run at 50hz #define USER_TEMPERATURE_LOOP user_temperature_logger(); // for code to be run at 10hz #define USER_HUMIDITY_LOOP user_humidity_logger(); // for code to be run at 3.3hz #define USER_WVANE_LOOP user_wvane_logger(); // for code to be run at 1hz diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index e0db599ae6fdf..444d6762bcd7b 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -175,7 +175,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(userhook_FastLoop, 100, 75), #endif #ifdef USER_VPBATT_LOOP - SCHED_TASK(user_vpbatt_monitor, 50, 75), + SCHED_TASK(user_vpbatt_monitor, 10, 75), #endif #ifdef USER_TEMPERATURE_LOOP SCHED_TASK(user_temperature_logger, 20, 75), From 60956feb7e62e2ad53a6cb6511867aec65ac5d77 Mon Sep 17 00:00:00 2001 From: tony2157 Date: Fri, 19 Jun 2020 14:00:18 -0500 Subject: [PATCH 03/15] Creation of new custom params for the VPBATT battery management function --- ArduCopter/Parameters.cpp | 23 ++++++++++++++++++++++- ArduCopter/Parameters.h | 18 ++++++++++++++---- 2 files changed, 36 insertions(+), 5 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 32178cb38b600..2d205e324a469 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -434,7 +434,7 @@ const AP_Param::Info Copter::var_info[] = { // @Description: Enable (1) or disable (0) wind vane mode auto RTL switch // @Range: 0 to 1 // @User: Advanced - GSCALAR(wind_vane_enabled, "WVANE_RTL_ON", 1.0f), + GSCALAR(wind_vane_enabled, "WVANE_RTL_EN", 1.0f), // @Param: USER_SENSORS // @DisplayName: Wind vane sampling frequency @@ -443,6 +443,27 @@ const AP_Param::Info Copter::var_info[] = { // @User: Advanced GSCALAR(wind_vane_fs, "WVANE_FS", 10.0f), + // @Param: USER_SENSORS + // @DisplayName: Wind vane sampling frequency + // @Description: Sampling and operating frequency of the wind vane algorithm + // @Range: 1 to 10 + // @User: Advanced + GSCALAR(vpbatt_enabled, "VPBATT_EN", 1.0f), + + // @Param: USER_SENSORS + // @DisplayName: Wind vane sampling frequency + // @Description: Sampling and operating frequency of the wind vane algorithm + // @Range: 1 to 10 + // @User: Advanced + GSCALAR(vpbatt_reserve, "VPBATT_RES", 30.0f), + + // @Param: USER_SENSORS + // @DisplayName: Wind vane sampling frequency + // @Description: Sampling and operating frequency of the wind vane algorithm + // @Range: 1 to 10 + // @User: Advanced + GSCALAR(vpbatt_wh, "VPBATT_WH", 89.2f), + ////////////////////////////CASS//////////////////////////////////// // @Param: LOG_BITMASK diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index e721b0cc83207..6ceaaba2d12c6 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -149,9 +149,14 @@ class Parameters { // 75: Singlecopter, CoaxCopter // k_param_single_servo_1 = 75, // remove - k_param_single_servo_2, // remove - k_param_single_servo_3, // remove - k_param_single_servo_4, // 78 - remove + // k_param_single_servo_2, // remove + // k_param_single_servo_3, // remove + // k_param_single_servo_4, // 78 - remove + + // CASS smart Battery monitor parameters + k_param_vpbatt_enabled, + k_param_vpbatt_reserve, + k_param_vpbatt_wh, // // 80: Heli @@ -459,7 +464,12 @@ class Parameters { AP_Float wind_vane_wsB; AP_Float wind_vane_spd_tol; AP_Float wind_vane_enabled; - AP_Float wind_vane_fs; + AP_Float wind_vane_fs; + + //CASS Vertical profiling smart Battery monitor params + AP_Float vpbatt_enabled; + AP_Float vpbatt_reserve; + AP_Float vpbatt_wh; // Misc // From cbd8805d071e17aa4442abefd46ba8bf2f893c45 Mon Sep 17 00:00:00 2001 From: tony2157 Date: Fri, 19 Jun 2020 14:01:04 -0500 Subject: [PATCH 04/15] Implementation of a smart battery management for vertical profiling --- ArduCopter/UserCode.cpp | 76 ++++++++++++++++++++++++++++++++++++----- 1 file changed, 67 insertions(+), 9 deletions(-) diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 9d555e48d3dcb..c362894a73399 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -20,7 +20,11 @@ float _wind_speed, _wind_dir; float R13, R23, R33; float last_yrate; bool high_wind_flag; -uint32_t last_now; +uint32_t wvane_now; +//Declare digital LPF +LPFrdFloat filt_thrvec_x; +LPFrdFloat filt_thrvec_y; +LPFrdFloat filt_thrvec_z; //g.wind_vane_wsA -> Coefficient A of the linear wind speed equation, from calibration //g.wind_vane_wsB -> Coefficient B of the linear wind speed equation, from calibration //g.wind_vane_min_roll -> Minimum roll angle that the wind vane will correct (too low and the copter will oscilate) @@ -28,10 +32,12 @@ uint32_t last_now; //g.wind_vane_fine_gain -> Wind vane gain: higher values will increase the resposivness //g.wind_vane_fs -> Wind vane sampling frequency, range 1 to 10Hz. -//Declare digital LPF -LPFrdFloat filt_thrvec_x; -LPFrdFloat filt_thrvec_y; -LPFrdFloat filt_thrvec_z; +//Smart Vertical Profiling Battery monitor parameters +float Whc; // Energy consumed +float Whn; // Energy needed to fly home safely +uint32_t vpbatt_now; +bool batt_home_ok; +bool batt_warning_flag; #ifdef USERHOOK_INIT void Copter::userhook_init() @@ -44,7 +50,7 @@ void Copter::userhook_init() R13 = 0.0f; R23 = 0.0f; last_yrate = 0; high_wind_flag = false; - last_now = AP_HAL::millis(); + wvane_now = AP_HAL::millis(); //Wind filter initialization float Fss; @@ -67,6 +73,12 @@ void Copter::userhook_init() filt_thrvec_z.set_cutoff_frequency(Fss,g.wind_vane_cutoff); } + //VPBatt_monitor initilize + Whc = Whn = 0; + batt_home_ok = true; + batt_warning_flag = false; + vpbatt_now = AP_HAL::millis(); + // Initialize Fan Control SRV_Channels::set_output_scaled(SRV_Channel::k_egg_drop, fan_pwm_off); _fan_status = false; @@ -83,7 +95,53 @@ void Copter::userhook_FastLoop() #ifdef USER_VPBATT_LOOP void Copter::user_vpbatt_monitor() { - // put your 50Hz code here + // Smart Battery Management - Use it only for Vertical profiling + float alt; + float dt = (float)(AP_HAL::millis() - vpbatt_now); + // Enter loop every 100 milliseconds + if(dt >= 100.0f){ + // Calculate energy consumed in Watt-hour + Whc = Whc + battery.voltage()*battery.current_amps()*dt/3.6e6f; + + // Calculate the Descent-Energy-consumption per meter height (function of wind speed) + float Whm = 3e-4f*_wind_speed + 8.5e-3; + // Constrain lower values + Whm = Whm > 0.01 ? Whm : 0.01; + + // Get current altitude + copter.ahrs.get_relative_position_D_home(alt); + alt = -1.0f*alt; + // Calculate energy needed to get home safely + Whn = Whm*alt; + + // Estimate the total energy used (percentage) + // vpbatt_reserve is the desired batt percentage after landing + float Wh_tot = (Whc + Whn)/g.vpbatt_wh + g.vpbatt_reserve/100.0f; + + //Switch to RTL automatically if battery reaches critical remaining energy + if(!is_zero(g.vpbatt_wh)){ + // Issue a warning once when the battery altitude range is over 85% + if(Wh_tot >= 0.85f && batt_warning_flag == false){ + // It will still warn, even if the function is disabled + if(!is_zero(g.vpbatt_enabled)){ + gcs().send_text(MAV_SEVERITY_WARNING, "Over 85 Batt range"); + } + batt_warning_flag = true; + } + + // Trigger RTL when max battery altitude range is reached + if(Wh_tot >= 1.0f && batt_home_ok == true){ + gcs().send_text(MAV_SEVERITY_WARNING, "Max Batt range: Switch to RTL"); + // It will still warn, even if the function is disabled + if(!is_zero(g.vpbatt_enabled)){ + copter.set_mode(RTL, MODE_REASON_UNKNOWN); + } + batt_home_ok = false; + } + } + // Update time + vpbatt_now = AP_HAL::millis(); + } } #endif @@ -239,7 +297,7 @@ void Copter::user_wvane_logger() R33 = -1*copter.ahrs.get_rotation_body_to_ned().c.z; //Wind vane loop starts here. Loop frequency is defined by WVANE_FS param in Hz - if((AP_HAL::millis() - last_now) >= (uint32_t)(1000/g.wind_vane_fs)){ + if((AP_HAL::millis() - wvane_now) >= (uint32_t)(1000/g.wind_vane_fs)){ //Apply Butterworth LPF on each element float thrvec_x, thrvec_y, thrvec_z; thrvec_x = filt_thrvec_x.apply(R13); @@ -319,7 +377,7 @@ void Copter::user_wvane_logger() } //Update last loop time - last_now = AP_HAL::millis(); + wvane_now = AP_HAL::millis(); } } From 56c573d839297a443f5e679a5df83b8826179e0a Mon Sep 17 00:00:00 2001 From: Huibean Date: Thu, 7 Nov 2019 22:40:06 +0800 Subject: [PATCH 05/15] fix waf build error with python3.7 --- wscript | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wscript b/wscript index 1749cd72cc774..442edaa008e0e 100644 --- a/wscript +++ b/wscript @@ -157,7 +157,7 @@ def _collect_autoconfig_files(cfg): paths.append(p) for p in paths: - if p in cfg.files or not os.path.isfile(p): + if p in cfg.files or not (p and os.path.isfile(p)): continue with open(p, 'rb') as f: From 98828ccf91477fc1a46ce3771ddb1efb8e8e9b14 Mon Sep 17 00:00:00 2001 From: tony2157 Date: Mon, 22 Jun 2020 17:03:12 -0500 Subject: [PATCH 06/15] VP battery managment is now a function of the wind speed memory (ot integral) and other modifications --- ArduCopter/UserCode.cpp | 93 ++++++++++++++++++++++++----------------- 1 file changed, 55 insertions(+), 38 deletions(-) diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index c362894a73399..4c3bc71013d77 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -35,6 +35,7 @@ LPFrdFloat filt_thrvec_z; //Smart Vertical Profiling Battery monitor parameters float Whc; // Energy consumed float Whn; // Energy needed to fly home safely +float int_wvspd; // Wind speed history or memory while ascending uint32_t vpbatt_now; bool batt_home_ok; bool batt_warning_flag; @@ -74,7 +75,7 @@ void Copter::userhook_init() } //VPBatt_monitor initilize - Whc = Whn = 0; + Whc = Whn = int_wvspd = 0; batt_home_ok = true; batt_warning_flag = false; vpbatt_now = AP_HAL::millis(); @@ -98,48 +99,64 @@ void Copter::user_vpbatt_monitor() // Smart Battery Management - Use it only for Vertical profiling float alt; float dt = (float)(AP_HAL::millis() - vpbatt_now); - // Enter loop every 100 milliseconds - if(dt >= 100.0f){ - // Calculate energy consumed in Watt-hour - Whc = Whc + battery.voltage()*battery.current_amps()*dt/3.6e6f; - - // Calculate the Descent-Energy-consumption per meter height (function of wind speed) - float Whm = 3e-4f*_wind_speed + 8.5e-3; - // Constrain lower values - Whm = Whm > 0.01 ? Whm : 0.01; - - // Get current altitude - copter.ahrs.get_relative_position_D_home(alt); - alt = -1.0f*alt; - // Calculate energy needed to get home safely - Whn = Whm*alt; - - // Estimate the total energy used (percentage) - // vpbatt_reserve is the desired batt percentage after landing - float Wh_tot = (Whc + Whn)/g.vpbatt_wh + g.vpbatt_reserve/100.0f; - - //Switch to RTL automatically if battery reaches critical remaining energy - if(!is_zero(g.vpbatt_wh)){ - // Issue a warning once when the battery altitude range is over 85% - if(Wh_tot >= 0.85f && batt_warning_flag == false){ - // It will still warn, even if the function is disabled - if(!is_zero(g.vpbatt_enabled)){ - gcs().send_text(MAV_SEVERITY_WARNING, "Over 85 Batt range"); + // Compute loop only when flying + if(!ap.land_complete){ + // Enter loop every 100 milliseconds + if(dt >= 100.0f){ + // Calculate energy consumed in Watt-hour + Whc = Whc + battery.voltage()*battery.current_amps()*dt/3.6e6f; + + //Compute the temporal integration of the wind speed (works as a memory) + int_wvspd = int_wvspd + _wind_speed*dt/1000; + + // Calculate the Descent-Energy-consumption per meter height (function of wind speed) + float Whm = 1.5e-6f*int_wvspd + 7e-3; + // Constrain lower values + Whm = Whm > 0.01 ? Whm : 0.01; + + // Get current altitude in meters + copter.ahrs.get_relative_position_D_home(alt); + alt = -1.0f*alt; + // Calculate energy needed to get home safely + Whn = Whm*alt; + + // Estimate the total energy used (percentage) + // vpbatt_reserve is the desired batt percentage after landing + float Wh_tot = (Whc + Whn)/g.vpbatt_wh + g.vpbatt_reserve/100.0f + 0.05f; + + //Switch to RTL automatically if battery reaches critical remaining energy + if(!is_zero(g.vpbatt_wh)){ + // Issue a warning once when the battery altitude range is over 85% + if(Wh_tot >= 0.85f && batt_warning_flag == false){ + // It will still warn, even if the function is disabled + if(!is_zero(g.vpbatt_enabled)){ + gcs().send_text(MAV_SEVERITY_WARNING, "Over 85 Batt range"); + } + batt_warning_flag = true; } - batt_warning_flag = true; - } - // Trigger RTL when max battery altitude range is reached - if(Wh_tot >= 1.0f && batt_home_ok == true){ - gcs().send_text(MAV_SEVERITY_WARNING, "Max Batt range: Switch to RTL"); - // It will still warn, even if the function is disabled - if(!is_zero(g.vpbatt_enabled)){ - copter.set_mode(RTL, MODE_REASON_UNKNOWN); + // Trigger RTL when max battery altitude range is reached + if(Wh_tot >= 1.0f && batt_home_ok == true){ + gcs().send_text(MAV_SEVERITY_WARNING, "Max Batt range: Switch to RTL"); + // It will still warn, even if the function is disabled + if(!is_zero(g.vpbatt_enabled)){ + copter.set_mode(RTL, MODE_REASON_UNKNOWN); + } + batt_home_ok = false; } - batt_home_ok = false; } + // Update time + vpbatt_now = AP_HAL::millis(); + + //Print on terminal for debugging + // printf("Whc: %5.2f \n",Whc); + // printf("int_wvspd: %5.4f \n",int_wvspd); + // printf("Whm: %5.4f \n",Whm); + // printf("Whn: %5.2f \n",Whn); + // printf("Wh_tot: %5.2f \n",Wh_tot); } - // Update time + } + else{ vpbatt_now = AP_HAL::millis(); } } From 58b448de71f3e10cdf986f64a07fc628ed9e5141 Mon Sep 17 00:00:00 2001 From: tony2157 Date: Wed, 24 Jun 2020 18:20:36 -0500 Subject: [PATCH 07/15] Whm only accumulates during the ascent --- ArduCopter/UserCode.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 4c3bc71013d77..4978a66052e32 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -107,7 +107,11 @@ void Copter::user_vpbatt_monitor() Whc = Whc + battery.voltage()*battery.current_amps()*dt/3.6e6f; //Compute the temporal integration of the wind speed (works as a memory) - int_wvspd = int_wvspd + _wind_speed*dt/1000; + Vector3f velocity; + copter.ahrs.get_velocity_NED(velocity); + if(velocity[2] < 0){ + int_wvspd = int_wvspd + _wind_speed*dt/1000; + } // Calculate the Descent-Energy-consumption per meter height (function of wind speed) float Whm = 1.5e-6f*int_wvspd + 7e-3; @@ -149,15 +153,17 @@ void Copter::user_vpbatt_monitor() vpbatt_now = AP_HAL::millis(); //Print on terminal for debugging - // printf("Whc: %5.2f \n",Whc); - // printf("int_wvspd: %5.4f \n",int_wvspd); - // printf("Whm: %5.4f \n",Whm); - // printf("Whn: %5.2f \n",Whn); - // printf("Wh_tot: %5.2f \n",Wh_tot); + printf("Whc: %5.2f \n",Whc); + printf("Vel_Z: %5.2f \n",velocity[2]); + printf("int_wvspd: %5.4f \n",int_wvspd); + printf("Whm: %5.4f \n",Whm); + printf("Whn: %5.2f \n",Whn); + printf("Wh_tot: %5.2f \n",Wh_tot); } } else{ vpbatt_now = AP_HAL::millis(); + int_wvspd = 0.0f; } } #endif From 5d6a66e38aa9d8a907e503063b538612eac7f4e8 Mon Sep 17 00:00:00 2001 From: tonysega Date: Fri, 26 Jun 2020 22:21:10 +0000 Subject: [PATCH 08/15] WVANE set different conditions for the ascent and descent --- ArduCopter/UserCode.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 4978a66052e32..6b68a49780024 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -359,13 +359,13 @@ void Copter::user_wvane_logger() _wind_speed = _wind_speed < 0 ? 0.0f : _wind_speed; //Get current velocity - Vector3f vel_xyz = copter.inertial_nav.get_velocity(); + Vector3f vel_xyz = copter.inertial_nav.get_velocity(); // NEU convention float tyaw = copter.wp_nav->get_yaw()*DEG_TO_RAD/100.0f; float speed_y = vel_xyz.y*cosf(tyaw) - vel_xyz.x*sinf(tyaw); float speed = norm(vel_xyz.x,vel_xyz.y); //Wind vane is active when flying horizontally steady and wind speed is perceivable - if(fabsf(speed_y) < 100.0f && _wind_speed > 1.0f){ + if(fabsf(speed_y) < 100.0f && _wind_speed > 1.0f && vel_xyz[2] >= 0){ //Min altitude and speed at which the yaw command is sent if(alt>400.0f && speed<(fabsf(speed_y)+100.0f)){ //Send estimated wind direction to the autopilot @@ -378,6 +378,18 @@ void Copter::user_wvane_logger() copter.cass_wind_speed = 0.0f; } } + else if (fabsf(speed_y) < 100.0f && _wind_speed > 2.0f && vel_xyz[2] < 0){ + if(alt>400.0f && speed<(fabsf(speed_y)+100.0f)){ + //Send estimated wind direction to the autopilot + copter.cass_wind_direction = _wind_dir; + copter.cass_wind_speed = _wind_speed; + } + else{ + //Send neutral values + copter.cass_wind_direction = copter.wp_nav->get_yaw(); + copter.cass_wind_speed = 0.0f; + } + } else{ //Reset 1st order filter last_yrate = 0.0f; From ef717accc3bd6524bf03a2715dbb9dee2e8d37f7 Mon Sep 17 00:00:00 2001 From: tony2157 Date: Sun, 28 Jun 2020 01:01:14 -0500 Subject: [PATCH 09/15] Adjust WVANE ascent-descent tolerances --- ArduCopter/UserCode.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 6b68a49780024..5eaa3a4c0e8c2 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -365,9 +365,10 @@ void Copter::user_wvane_logger() float speed = norm(vel_xyz.x,vel_xyz.y); //Wind vane is active when flying horizontally steady and wind speed is perceivable - if(fabsf(speed_y) < 100.0f && _wind_speed > 1.0f && vel_xyz[2] >= 0){ + //Condition when ascending + if(fabsf(speed_y) < 100.0f && _wind_speed > 1.0f && vel_xyz[2] >= 0.0f){ //Min altitude and speed at which the yaw command is sent - if(alt>400.0f && speed<(fabsf(speed_y)+100.0f)){ + if(alt>300.0f && speed<(fabsf(speed_y)+100.0f)){ //Send estimated wind direction to the autopilot copter.cass_wind_direction = _wind_dir; copter.cass_wind_speed = _wind_speed; @@ -378,8 +379,9 @@ void Copter::user_wvane_logger() copter.cass_wind_speed = 0.0f; } } - else if (fabsf(speed_y) < 100.0f && _wind_speed > 2.0f && vel_xyz[2] < 0){ - if(alt>400.0f && speed<(fabsf(speed_y)+100.0f)){ + //Condition when descending + else if (fabsf(speed_y) < 100.0f && _wind_speed > 3.0f && vel_xyz[2] < 0.0f){ + if(alt>800.0f && speed<(fabsf(speed_y)+100.0f)){ //Send estimated wind direction to the autopilot copter.cass_wind_direction = _wind_dir; copter.cass_wind_speed = _wind_speed; From d8ef8bf799c1cc39dbdeef48229f9a1e8961dea1 Mon Sep 17 00:00:00 2001 From: tony2157 Date: Wed, 8 Jul 2020 18:33:45 -0500 Subject: [PATCH 10/15] Reduce IMET ADC sampling rate to match the rest of the sensors --- ArduCopter/ArduCopter.cpp | 2 +- libraries/AC_CASS_IMET/AC_CASS_Imet.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 444d6762bcd7b..864ee689edc47 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -178,7 +178,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(user_vpbatt_monitor, 10, 75), #endif #ifdef USER_TEMPERATURE_LOOP - SCHED_TASK(user_temperature_logger, 20, 75), + SCHED_TASK(user_temperature_logger, 10, 75), #endif #ifdef USER_HUMIDITY_LOOP SCHED_TASK(user_humidity_logger, 10, 75), diff --git a/libraries/AC_CASS_IMET/AC_CASS_Imet.cpp b/libraries/AC_CASS_IMET/AC_CASS_Imet.cpp index bcb1e9861ea2b..cf857404bab35 100644 --- a/libraries/AC_CASS_IMET/AC_CASS_Imet.cpp +++ b/libraries/AC_CASS_IMET/AC_CASS_Imet.cpp @@ -44,7 +44,7 @@ bool AC_CASS_Imet::init(uint8_t busId, uint8_t i2cAddr) ADS1115_REG_CONFIG_CLAT_NONLAT | // Non-latching (default val) ADS1115_REG_CONFIG_CPOL_ACTVLOW | // Alert/Rdy active low (default val) ADS1115_REG_CONFIG_CMODE_TRAD | // Traditional comparator (default val) - ADS1115_REG_CONFIG_DR_32SPS | // 32 samples (conversions) per second + ADS1115_REG_CONFIG_DR_16SPS | // 16 samples (conversions) per second ADS1115_REG_CONFIG_MODE_SINGLE | // Single-shot mode (default) ADS1115_REG_CONFIG_PGA_6_144V | // Set PGA/voltage range ADS1115_REG_CONFIG_OS_SINGLE; // Start single-conversion @@ -80,7 +80,7 @@ bool AC_CASS_Imet::init(uint8_t busId, uint8_t i2cAddr) _dev->get_semaphore()->give(); // Register sensor to the I2C manager - _dev->register_periodic_callback(50000, + _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AC_CASS_Imet::_timer, void)); return true; } From 75d54d9560775a48aeb533a40b44cf033b933eca Mon Sep 17 00:00:00 2001 From: tony2157 Date: Fri, 17 Jul 2020 21:36:09 -0500 Subject: [PATCH 11/15] Enable User Switch functions --- ArduCopter/APM_Config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index e5243c3eea4a9..a0b044cd41923 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -110,4 +110,4 @@ #define USER_TEMPERATURE_LOOP user_temperature_logger(); // for code to be run at 10hz #define USER_HUMIDITY_LOOP user_humidity_logger(); // for code to be run at 3.3hz #define USER_WVANE_LOOP user_wvane_logger(); // for code to be run at 1hz -//#define USERHOOK_AUXSWITCH ENABLED // for code to handle user aux switches +#define USERHOOK_AUXSWITCH ENABLED // for code to handle user aux switches From 8397ee5eaad3ab39c4e520d3b3892d64a68aa646 Mon Sep 17 00:00:00 2001 From: tony2157 Date: Fri, 17 Jul 2020 21:37:21 -0500 Subject: [PATCH 12/15] Add parameter to set the target altitude for a vertical profiling flight --- ArduCopter/Parameters.cpp | 7 +++++++ ArduCopter/Parameters.h | 8 +++++++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 2d205e324a469..2a2404b1cf8f1 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -464,6 +464,13 @@ const AP_Param::Info Copter::var_info[] = { // @User: Advanced GSCALAR(vpbatt_wh, "VPBATT_WH", 89.2f), + // @Param: USER_SENSORS + // @DisplayName: Wind vane sampling frequency + // @Description: Sampling and operating frequency of the wind vane algorithm + // @Range: 1 to 10 + // @User: Advanced + GSCALAR(autovp_max_altitude, "AUTOVP_ALT", 120.0f), + ////////////////////////////CASS//////////////////////////////////// // @Param: LOG_BITMASK diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 6ceaaba2d12c6..8c4d21b5e0d74 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -148,11 +148,14 @@ class Parameters { // // 75: Singlecopter, CoaxCopter // - k_param_single_servo_1 = 75, // remove + // k_param_single_servo_1 = 75, // remove // k_param_single_servo_2, // remove // k_param_single_servo_3, // remove // k_param_single_servo_4, // 78 - remove + // CASS AutoVP mission generation + k_param_autovp_max_altitude = 75, + // CASS smart Battery monitor parameters k_param_vpbatt_enabled, k_param_vpbatt_reserve, @@ -471,6 +474,9 @@ class Parameters { AP_Float vpbatt_reserve; AP_Float vpbatt_wh; + //CASS AutoVP mission auto-generation + AP_Float autovp_max_altitude; + // Misc // AP_Int32 log_bitmask; From 68e25d3fc8b023cd6b2f6e5059283795813c78e1 Mon Sep 17 00:00:00 2001 From: tony2157 Date: Fri, 17 Jul 2020 21:38:22 -0500 Subject: [PATCH 13/15] Write code for auto generation of waypoints for vertical profiles --- ArduCopter/UserCode.cpp | 108 ++++++++++++++++++++++++++++++++++++++-- 1 file changed, 104 insertions(+), 4 deletions(-) diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 5eaa3a4c0e8c2..b3442bee89ac2 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -40,6 +40,9 @@ uint32_t vpbatt_now; bool batt_home_ok; bool batt_warning_flag; +//AutoVP mission generation +uint32_t mission_now; + #ifdef USERHOOK_INIT void Copter::userhook_init() { @@ -53,6 +56,9 @@ void Copter::userhook_init() high_wind_flag = false; wvane_now = AP_HAL::millis(); + //AutoVP initialize + mission_now = AP_HAL::millis(); + //Wind filter initialization float Fss; if(is_zero(fmodf(10,g.wind_vane_fs))){ @@ -368,7 +374,7 @@ void Copter::user_wvane_logger() //Condition when ascending if(fabsf(speed_y) < 100.0f && _wind_speed > 1.0f && vel_xyz[2] >= 0.0f){ //Min altitude and speed at which the yaw command is sent - if(alt>300.0f && speed<(fabsf(speed_y)+100.0f)){ + if(alt>500.0f && speed<(fabsf(speed_y)+100.0f)){ //Send estimated wind direction to the autopilot copter.cass_wind_direction = _wind_dir; copter.cass_wind_speed = _wind_speed; @@ -381,14 +387,13 @@ void Copter::user_wvane_logger() } //Condition when descending else if (fabsf(speed_y) < 100.0f && _wind_speed > 3.0f && vel_xyz[2] < 0.0f){ - if(alt>800.0f && speed<(fabsf(speed_y)+100.0f)){ + if(alt>1000.0f && speed<(fabsf(speed_y)+100.0f)){ //Send estimated wind direction to the autopilot copter.cass_wind_direction = _wind_dir; copter.cass_wind_speed = _wind_speed; } else{ - //Send neutral values - copter.cass_wind_direction = copter.wp_nav->get_yaw(); + //Do nothing - keep yaw equal to last wind direction estimate copter.cass_wind_speed = 0.0f; } } @@ -450,6 +455,101 @@ void Copter::user_wvane_logger() void Copter::userhook_auxSwitch1(uint8_t ch_flag) { // put your aux switch #1 handler here (CHx_OPT = 47) + + AP_Mission::Mission_Command cmd; + int32_t vp_lat = copter.current_loc.lat; // ahrs.get_home().lat; + int32_t vp_lng = copter.current_loc.lng; // ahrs.get_home().lng; + float max_alt = g.autovp_max_altitude*100; //convert to cm + char autovp_message[26]; + + // Run code when switch is toggled HIGH (pwm > 1800) + if(ch_flag==AUX_SWITCH_HIGH){ + // Check if drone is grounded and ready to create a mission + if(ap.land_complete && copter.position_ok() && (AP_HAL::millis() - mission_now) > 5000){ + + // clear mission + mission.clear(); + + // Command #0 : home + cmd.id = MAV_CMD_NAV_WAYPOINT; + cmd.content.location.options = 0; + cmd.p1 = 0; + cmd.content.location.flags.relative_alt = 1; + cmd.content.location.alt = 0; + cmd.content.location.lat = vp_lat; + cmd.content.location.lng = vp_lng; + if (!mission.add_cmd(cmd)) { + gcs().send_text(MAV_SEVERITY_WARNING, "AutoVP: failed to create mission"); + } + + // Command #1 : take-off to 2m + cmd.id = MAV_CMD_NAV_TAKEOFF; + cmd.content.location.options = 0; + cmd.p1 = 0; + cmd.content.location.flags.relative_alt = 1; + cmd.content.location.alt = 200; //in cm + cmd.content.location.lat = 0; + cmd.content.location.lng = 0; + if (!mission.add_cmd(cmd)) { + gcs().send_text(MAV_SEVERITY_WARNING, "AutoVP: failed to create mission"); + } + + // Command #2 : Bottom waypoint at 10m + cmd.id = MAV_CMD_NAV_WAYPOINT; + cmd.content.location.options = 0; + cmd.p1 = 0; + cmd.content.location.flags.relative_alt = 1; + cmd.content.location.alt = 1000; + cmd.content.location.lat = vp_lat; + cmd.content.location.lng = vp_lng; + if (!mission.add_cmd(cmd)) { + gcs().send_text(MAV_SEVERITY_WARNING, "AutoVP: failed to create mission"); + } + + // Constrain target altitude + if(max_alt > 180000){ + max_alt = 180000; + gcs().send_text(MAV_SEVERITY_INFO, "AutoVP: Max Alt set to 1500m"); + } + if(max_alt < 1000){ + max_alt = 1000; + gcs().send_text(MAV_SEVERITY_INFO, "AutoVP: Max Alt set to 10m"); + } + + // Command #3 : Top waypoint at desired altitude (from parameter) + cmd.id = MAV_CMD_NAV_WAYPOINT; + cmd.content.location.options = 0; + cmd.p1 = 0; + cmd.content.location.flags.relative_alt = 1; + cmd.content.location.alt = (int32_t)max_alt; + cmd.content.location.lat = vp_lat; + cmd.content.location.lng = vp_lng; + if (!mission.add_cmd(cmd)) { + gcs().send_text(MAV_SEVERITY_WARNING, "AutoVP: failed to create mission"); + } + + // Command #4 : RTL + cmd.id = MAV_CMD_NAV_RETURN_TO_LAUNCH; + cmd.p1 = 0; + cmd.content.location.flags.relative_alt = 1; + cmd.content.location.lat = 0; + cmd.content.location.lng = 0; + cmd.content.location.alt = 0; + if (!mission.add_cmd(cmd)) { + gcs().send_text(MAV_SEVERITY_WARNING, "AutoVP: failed to create mission"); + } + + // Send successful creation message + snprintf(autovp_message, 26, "AutoVP target alt: %4.0f m",max_alt/100); + gcs().send_text(MAV_SEVERITY_INFO, autovp_message); + + mission_now = AP_HAL::millis(); + } + else{ + // Send unable to create mission message warning + gcs().send_text(MAV_SEVERITY_WARNING, "AutoVP: Unable to create mission"); + } + } } void Copter::userhook_auxSwitch2(uint8_t ch_flag) From cec38bc09b58e581186b62ce0540a3442e3b6e87 Mon Sep 17 00:00:00 2001 From: tony2157 Date: Sat, 18 Jul 2020 23:13:04 -0500 Subject: [PATCH 14/15] AutoVP Minor changes in the info messages displayed --- ArduCopter/UserCode.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index b3442bee89ac2..e0f15e709c543 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -460,7 +460,7 @@ void Copter::userhook_auxSwitch1(uint8_t ch_flag) int32_t vp_lat = copter.current_loc.lat; // ahrs.get_home().lat; int32_t vp_lng = copter.current_loc.lng; // ahrs.get_home().lng; float max_alt = g.autovp_max_altitude*100; //convert to cm - char autovp_message[26]; + char autovp_message[22]; // Run code when switch is toggled HIGH (pwm > 1800) if(ch_flag==AUX_SWITCH_HIGH){ @@ -482,12 +482,12 @@ void Copter::userhook_auxSwitch1(uint8_t ch_flag) gcs().send_text(MAV_SEVERITY_WARNING, "AutoVP: failed to create mission"); } - // Command #1 : take-off to 2m + // Command #1 : take-off to 5m cmd.id = MAV_CMD_NAV_TAKEOFF; cmd.content.location.options = 0; cmd.p1 = 0; cmd.content.location.flags.relative_alt = 1; - cmd.content.location.alt = 200; //in cm + cmd.content.location.alt = 500; //in cm cmd.content.location.lat = 0; cmd.content.location.lng = 0; if (!mission.add_cmd(cmd)) { @@ -509,7 +509,7 @@ void Copter::userhook_auxSwitch1(uint8_t ch_flag) // Constrain target altitude if(max_alt > 180000){ max_alt = 180000; - gcs().send_text(MAV_SEVERITY_INFO, "AutoVP: Max Alt set to 1500m"); + gcs().send_text(MAV_SEVERITY_INFO, "AutoVP: Max Alt set to 1800m"); } if(max_alt < 1000){ max_alt = 1000; @@ -540,7 +540,8 @@ void Copter::userhook_auxSwitch1(uint8_t ch_flag) } // Send successful creation message - snprintf(autovp_message, 26, "AutoVP target alt: %4.0f m",max_alt/100); + gcs().send_text(MAV_SEVERITY_INFO, "AutoVP mission received"); + snprintf(autovp_message, 22, "Target alt: %.1f m",max_alt/100); gcs().send_text(MAV_SEVERITY_INFO, autovp_message); mission_now = AP_HAL::millis(); From 2bc433ea434b85973d8aa530e9cb9a7d55225ce8 Mon Sep 17 00:00:00 2001 From: tony2157 Date: Tue, 28 Jul 2020 16:52:37 -0500 Subject: [PATCH 15/15] Fix bug in imet resistance model --- ArduCopter/GCS_Mavlink.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index b1211365496b1..bcf63f7cffb25 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -234,15 +234,16 @@ void Copter::send_cass_imet(mavlink_channel_t chan) { if(!HAVE_PAYLOAD_SPACE(chan, CASS_SENSOR_RAW)){ return; } - for(uint8_t i=0; i<4; i++){ - raw_sensor[i] = copter.CASS_Imet[i].resistance(); - } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - // Variables simulation for IMET resistance measurements + // Variables simulation for IMET resistance measurements for(uint8_t i=0; i<4; i++){ raw_sensor[i] = -356.9892f*raw_sensor[i] + 110935.3763f; } //printf("HYT271 temp: %5.2f \n",raw_sensor[0]); + #else + for(uint8_t i=0; i<4; i++){ + raw_sensor[i] = copter.CASS_Imet[i].resistance(); + } #endif // Call Mavlink function and send CASS data mavlink_msg_cass_sensor_raw_send(