From d197ec4072b1a8e149534b7aa307dde5d21dd15b Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 18 Jul 2020 23:01:36 +0100 Subject: [PATCH] Sub: add airspeed sensors --- ArduSub/ArduSub.cpp | 1 + ArduSub/Parameters.cpp | 7 ++++++- ArduSub/Parameters.h | 3 +++ ArduSub/Sub.h | 1 + ArduSub/sensors.cpp | 8 ++++++++ ArduSub/system.cpp | 2 ++ 6 files changed, 21 insertions(+), 1 deletion(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 7b5150cccd..2de8ee36c9 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -78,6 +78,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { #ifdef USERHOOK_SUPERSLOWLOOP SCHED_TASK(userhook_SuperSlowLoop, 1, 75), #endif + SCHED_TASK(read_airspeed, 10, 100), }; void Sub::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index efbedce2eb..360b7f3856 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -632,13 +632,18 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(scripting, "SCR_", 18, ParametersG2, AP_Scripting), #endif + // @Group: ARSPD + // @Path: ../libraries/AP_Airspeed/AP_Airspeed.cpp + AP_SUBGROUPINFO(airspeed, "ARSPD", 19, ParametersG2, AP_Airspeed), + AP_GROUPEND }; /* constructor for g2 object */ -ParametersG2::ParametersG2() +ParametersG2::ParametersG2(): + airspeed() { AP_Param::setup_object_defaults(this, var_info); } diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 12a9d98009..475b498cd7 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -335,6 +335,9 @@ public: AP_Scripting scripting; #endif // ENABLE_SCRIPTING + // Airspeed + AP_Airspeed airspeed; + }; extern const AP_Param::Info var_info[]; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 7b95c31b76..ac45645af7 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -610,6 +610,7 @@ private: void auto_spline_start(const Location& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location& next_destination); void log_init(void); void accel_cal_update(void); + void read_airspeed(); void failsafe_leak_check(); void failsafe_internal_pressure_check(); diff --git a/ArduSub/sensors.cpp b/ArduSub/sensors.cpp index 51dc1e8ed4..5cfc448b56 100644 --- a/ArduSub/sensors.cpp +++ b/ArduSub/sensors.cpp @@ -107,3 +107,11 @@ void Sub::accel_cal_update() ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); } } + +/* + ask airspeed sensor for a new value, duplicated from plane + */ +void Sub::read_airspeed() +{ + g2.airspeed.update(should_log(MASK_LOG_IMU)); +} diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 08505fc15a..546d3bb6bd 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -165,6 +165,8 @@ void Sub::init_ardupilot() g2.scripting.init(); #endif // ENABLE_SCRIPTING + g2.airspeed.init(); + // we don't want writes to the serial port to cause us to pause // mid-flight, so set the serial ports non-blocking once we are // ready to fly