diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 8463fa68f2..55ddf0304d 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -128,6 +128,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] PROGMEM = { { SCHED_TASK(full_rate_logging_loop),1, 100 }, { SCHED_TASK(perf_update), 4000, 75 }, { SCHED_TASK(read_receiver_rssi), 40, 75 }, + { SCHED_TASK(rpm_update), 40, 200 }, #if FRSKY_TELEM_ENABLED == ENABLED { SCHED_TASK(frsky_telemetry_send), 80, 75 }, #endif diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index d19ca04bd3..05c73ba70f 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -102,6 +102,7 @@ #endif #include // Landing Gear library #include +#include // AP_HAL to Arduino compatibility layer // Configuration @@ -172,6 +173,8 @@ private: bool sonar_enabled; // enable user switch for sonar #endif + AP_RPM rpm_sensor; + // Inertial Navigation EKF NavEKF EKF{&ahrs, barometer, sonar}; AP_AHRS_NavEKF ahrs{ins, barometer, gps, sonar, EKF}; @@ -579,6 +582,8 @@ private: void send_vfr_hud(mavlink_channel_t chan); void send_current_waypoint(mavlink_channel_t chan); void send_rangefinder(mavlink_channel_t chan); + void send_rpm(mavlink_channel_t chan); + void rpm_update(); void send_pid_tuning(mavlink_channel_t chan); void send_statustext(mavlink_channel_t chan); bool telemetry_delayed(mavlink_channel_t chan); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index a6442736cd..858ab145fd 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -414,6 +414,19 @@ void NOINLINE Copter::send_rangefinder(mavlink_channel_t chan) } #endif +/* + send RPM packet + */ +void NOINLINE Copter::send_rpm(mavlink_channel_t chan) +{ + if (rpm_sensor.healthy(0) || rpm_sensor.healthy(1)) { + mavlink_msg_rpm_send( + chan, + rpm_sensor.get_rpm(0), + rpm_sensor.get_rpm(1)); + } +} + /* send PID tuning message @@ -629,6 +642,11 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) #endif break; + case MSG_RPM: + CHECK_PAYLOAD_SIZE(RPM); + copter.send_rpm(chan); + break; + case MSG_TERRAIN: #if AP_TERRAIN_AVAILABLE CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST); @@ -945,6 +963,7 @@ GCS_MAVLINK::data_stream_send(void) send_message(MSG_GIMBAL_REPORT); send_message(MSG_EKF_STATUS_REPORT); send_message(MSG_VIBRATION); + send_message(MSG_RPM); } } diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 238c792c30..2c1811f88f 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1046,6 +1046,10 @@ const AP_Param::Info Copter::var_info[] PROGMEM = { GOBJECT(optflow, "FLOW", OpticalFlow), #endif + // @Group: RPM + // @Path: ../libraries/AP_RPM/AP_RPM.cpp + GOBJECT(rpm_sensor, "RPM", AP_RPM), + // @Param: AUTOTUNE_AXIS_BITMASK // @DisplayName: Autotune axis bitmask // @Description: 1-byte bitmap of axes to autotune diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 40e2d0c77c..08cf3ab382 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -337,6 +337,7 @@ public: k_param_pi_vel_xy, k_param_fs_ekf_action, k_param_rtl_climb_min, // 249 + k_param_rpm_sensor, // 254,255: reserved }; diff --git a/ArduCopter/make.inc b/ArduCopter/make.inc index 834f7ea0ca..b0689a2187 100644 --- a/ArduCopter/make.inc +++ b/ArduCopter/make.inc @@ -63,3 +63,4 @@ LIBRARIES += AP_EPM LIBRARIES += AP_Parachute LIBRARIES += AP_LandingGear LIBRARIES += AP_Terrain +LIBRARIES += AP_RPM diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index d6ed0cc0f2..0b02778ac8 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -69,6 +69,14 @@ int16_t Copter::read_sonar(void) #endif } +/* + update RPM sensors + */ +void Copter::rpm_update(void) +{ + rpm_sensor.update(); +} + // initialise compass void Copter::init_compass() { diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 479af37580..69c60a7119 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -240,6 +240,9 @@ void Copter::init_ardupilot() init_sonar(); #endif + // initialise AP_RPM library + rpm_sensor.init(); + // initialise mission library mission.init();