Merge pull request #29 from tony2157/Copter-3.6

CASSv1.6.1 updates
oubliss · Aug 6, 2020 · 2902fff · 2902fff
2 parents 53e3304 + 2bc433e
commit 2902fff
Show file tree

Hide file tree

Showing 9 changed files with 271 additions and 30 deletions.
diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h
@@ -106,8 +106,8 @@
 // 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
-//#define USERHOOK_AUXSWITCH ENABLED // for code to handle user aux switches
+#define USERHOOK_AUXSWITCH ENABLED // for code to handle user aux switches
diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp
@@ -175,10 +175,10 @@ 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),
+ SCHED_TASK(user_temperature_logger, 10, 75),
 #endif
 #ifdef USER_HUMIDITY_LOOP
  SCHED_TASK(user_humidity_logger, 10, 75),

diff --git 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(

diff --git 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,34 @@ 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),
+
+ // @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
@@ -148,10 +148,18 @@ 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_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,
+ k_param_vpbatt_wh,
 
  //
  // 80: Heli
@@ -459,7 +467,15 @@ 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;
+
+ //CASS AutoVP mission auto-generation
+ AP_Float autovp_max_altitude;
 
  // Misc
  //