Copter: Add option to disable RPM module

This commit is contained in:
Dr.-Ing. Amilcar do Carmo Lucas 2018-03-29 15:49:27 +02:00 committed by Randy Mackay
parent 2f59572d42
commit d633ba8c46
8 changed files with 24 additions and 1 deletions

View File

@ -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

View File

@ -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),

View File

@ -77,7 +77,6 @@
#include <AP_BoardConfig/AP_BoardConfig.h> // board configuration library
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#include <AP_LandingGear/AP_LandingGear.h> // Landing Gear library
#include <AP_RPM/AP_RPM.h>
#include <AC_InputManager/AC_InputManager.h> // Pilot input handling library
#include <AC_InputManager/AC_InputManager_Heli.h> // Heli specific pilot input handling library
#include <AP_Button/AP_Button.h>
@ -157,6 +156,9 @@
# include <AP_WheelEncoder/AP_WheelEncoder.h>
# include <AP_Winch/AP_Winch.h>
#endif
#if RPM_ENABLED == ENABLED
#include <AP_RPM/AP_RPM.h>
#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};

View File

@ -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:

View File

@ -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_

View File

@ -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

View File

@ -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

View File

@ -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