diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 6ef3d97b96..00145a50d3 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -25,6 +25,7 @@ //#define SPRAYER DISABLED // disable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity) //#define WINCH_ENABLED DISABLED // disable winch support //#define GRIPPER_ENABLED DISABLED // disable gripper support +//#define RPM_ENABLED DISABLED // disable rotations per minute sensor support //#define MAGNETOMETER DISABLED // disable magnetometer support //#define STATS_ENABLED DISABLED // disable statistics support //#define MODE_ACRO_ENABLED DISABLED // disable acrobatic mode support diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index f9b001949a..5026a1af0d 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -151,7 +151,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50), SCHED_TASK_CLASS(AP_Scheduler, &copter.scheduler, update_logging, 0.1, 75), SCHED_TASK(read_receiver_rssi, 10, 75), +#if RPM_ENABLED == ENABLED SCHED_TASK(rpm_update, 10, 200), +#endif SCHED_TASK(compass_cal_update, 100, 100), SCHED_TASK(accel_cal_update, 10, 100), SCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, update, 10, 100), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 43dc4549bc..1d71504ece 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -77,7 +77,6 @@ #include // board configuration library #include #include // Landing Gear library -#include #include // Pilot input handling library #include // Heli specific pilot input handling library #include @@ -157,6 +156,9 @@ # include # include #endif +#if RPM_ENABLED == ENABLED + #include +#endif // Local modules #include "Parameters.h" @@ -237,7 +239,9 @@ private: int8_t glitch_count; } rangefinder_state = { false, false, 0, 0 }; +#if RPM_ENABLED == ENABLED AP_RPM rpm_sensor; +#endif // Inertial Navigation EKF NavEKF2 EKF2{&ahrs, rangefinder}; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index b69613e3c5..63f09e75c8 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -190,12 +190,14 @@ void NOINLINE Copter::send_vfr_hud(mavlink_channel_t chan) */ void NOINLINE Copter::send_rpm(mavlink_channel_t chan) { +#if RPM_ENABLED == ENABLED if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) { mavlink_msg_rpm_send( chan, rpm_sensor.get_rpm(0), rpm_sensor.get_rpm(1)); } +#endif } @@ -366,8 +368,10 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id) break; case MSG_RPM: +#if RPM_ENABLED == ENABLED CHECK_PAYLOAD_SIZE(RPM); copter.send_rpm(chan); +#endif break; case MSG_TERRAIN: diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 79edcd9856..11aee0fe16 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -737,9 +737,11 @@ const AP_Param::Info Copter::var_info[] = { GOBJECT(precland, "PLND_", AC_PrecLand), #endif +#if RPM_ENABLED == ENABLED // @Group: RPM // @Path: ../libraries/AP_RPM/AP_RPM.cpp GOBJECT(rpm_sensor, "RPM", AP_RPM), +#endif #if ADSB_ENABLED == ENABLED // @Group: ADSB_ diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 7f6235a825..68470a106c 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -241,6 +241,12 @@ # define WINCH_ENABLED !HAL_MINIMIZE_FEATURES #endif +////////////////////////////////////////////////////////////////////////////// +// rotations per minute sensor support +#ifndef RPM_ENABLED +# define RPM_ENABLED ENABLED +#endif + ////////////////////////////////////////////////////////////////////////////// // Parachute release #ifndef PARACHUTE diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index b1b85fe5cf..a2739d6073 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -89,12 +89,14 @@ bool Copter::rangefinder_alt_ok() */ void Copter::rpm_update(void) { +#if RPM_ENABLED == ENABLED rpm_sensor.update(); if (rpm_sensor.enabled(0) || rpm_sensor.enabled(1)) { if (should_log(MASK_LOG_RCIN)) { DataFlash.Log_Write_RPM(rpm_sensor); } } +#endif } // initialise compass diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 3b19e918f8..bc4a9cf905 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -225,8 +225,10 @@ void Copter::init_ardupilot() // init visual odometry init_visual_odom(); +#if RPM_ENABLED == ENABLED // initialise AP_RPM library rpm_sensor.init(); +#endif #if MODE_AUTO_ENABLED == ENABLED // initialise mission library