ArduCopter: add AP_GRIPPER_ENABLED

This commit is contained in:
Peter Barker 2022-09-20 17:37:48 +10:00 committed by Andrew Tridgell
parent 61c4320e89
commit be93d8212c
10 changed files with 12 additions and 17 deletions

View File

@ -13,7 +13,6 @@
//#define BEACON_ENABLED DISABLED // disable beacon support //#define BEACON_ENABLED DISABLED // disable beacon support
//#define SPRAYER_ENABLED DISABLED // disable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity) //#define SPRAYER_ENABLED 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 WINCH_ENABLED DISABLED // disable winch support
//#define GRIPPER_ENABLED DISABLED // disable gripper support
//#define STATS_ENABLED DISABLED // disable statistics support //#define STATS_ENABLED DISABLED // disable statistics support
//#define MODE_ACRO_ENABLED DISABLED // disable acrobatic mode support //#define MODE_ACRO_ENABLED DISABLED // disable acrobatic mode support
//#define MODE_AUTO_ENABLED DISABLED // disable auto mode support //#define MODE_AUTO_ENABLED DISABLED // disable auto mode support

View File

@ -227,7 +227,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
SCHED_TASK(terrain_update, 10, 100, 144), SCHED_TASK(terrain_update, 10, 100, 144),
#endif #endif
#if GRIPPER_ENABLED == ENABLED #if AP_GRIPPER_ENABLED
SCHED_TASK_CLASS(AP_Gripper, &copter.g2.gripper, update, 10, 75, 147), SCHED_TASK_CLASS(AP_Gripper, &copter.g2.gripper, update, 10, 75, 147),
#endif #endif
#if WINCH_ENABLED == ENABLED #if WINCH_ENABLED == ENABLED

View File

@ -109,7 +109,8 @@
#include <AC_WPNav/AC_WPNav_OA.h> #include <AC_WPNav/AC_WPNav_OA.h>
#include <AC_Avoidance/AP_OAPathPlanner.h> #include <AC_Avoidance/AP_OAPathPlanner.h>
#endif #endif
#if GRIPPER_ENABLED == ENABLED #include <AP_Gripper/AP_Gripper_config.h>
#if AP_GRIPPER_ENABLED
# include <AP_Gripper/AP_Gripper.h> # include <AP_Gripper/AP_Gripper.h>
#endif #endif
#if PRECISION_LANDING == ENABLED #if PRECISION_LANDING == ENABLED

View File

@ -825,7 +825,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_SUBGROUPINFO(stats, "STAT", 12, ParametersG2, AP_Stats), AP_SUBGROUPINFO(stats, "STAT", 12, ParametersG2, AP_Stats),
#endif #endif
#if GRIPPER_ENABLED == ENABLED #if AP_GRIPPER_ENABLED
// @Group: GRIP_ // @Group: GRIP_
// @Path: ../libraries/AP_Gripper/AP_Gripper.cpp // @Path: ../libraries/AP_Gripper/AP_Gripper.cpp
AP_SUBGROUPINFO(gripper, "GRIP_", 13, ParametersG2, AP_Gripper), AP_SUBGROUPINFO(gripper, "GRIP_", 13, ParametersG2, AP_Gripper),

View File

@ -4,7 +4,8 @@
#include "RC_Channel.h" #include "RC_Channel.h"
#include <AP_Proximity/AP_Proximity.h> #include <AP_Proximity/AP_Proximity.h>
#if GRIPPER_ENABLED == ENABLED #include <AP_Gripper/AP_Gripper_config.h>
#if AP_GRIPPER_ENABLED
# include <AP_Gripper/AP_Gripper.h> # include <AP_Gripper/AP_Gripper.h>
#endif #endif
#if MODE_FOLLOW_ENABLED == ENABLED #if MODE_FOLLOW_ENABLED == ENABLED
@ -502,7 +503,7 @@ public:
AP_Stats stats; AP_Stats stats;
#endif #endif
#if GRIPPER_ENABLED #if AP_GRIPPER_ENABLED
AP_Gripper gripper; AP_Gripper gripper;
#endif #endif

View File

@ -181,12 +181,6 @@
# define PRECISION_LANDING ENABLED # define PRECISION_LANDING ENABLED
#endif #endif
//////////////////////////////////////////////////////////////////////////////
// gripper - enabled only on larger firmwares
#ifndef GRIPPER_ENABLED
# define GRIPPER_ENABLED !HAL_MINIMIZE_FEATURES
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// winch support // winch support
#ifndef WINCH_ENABLED #ifndef WINCH_ENABLED

View File

@ -170,7 +170,7 @@ void Copter::thrust_loss_check()
motors->set_thrust_boost(true); motors->set_thrust_boost(true);
// the motors library disables this when it is no longer needed to achieve the commanded output // the motors library disables this when it is no longer needed to achieve the commanded output
#if GRIPPER_ENABLED == ENABLED #if AP_GRIPPER_ENABLED
if ((copter.g2.flight_options & uint32_t(FlightOptions::RELEASE_GRIPPER_ON_THRUST_LOSS)) != 0) { if ((copter.g2.flight_options & uint32_t(FlightOptions::RELEASE_GRIPPER_ON_THRUST_LOSS)) != 0) {
copter.g2.gripper.release(); copter.g2.gripper.release();
} }

View File

@ -478,7 +478,7 @@ void Copter::do_failsafe_action(FailsafeAction action, ModeReason reason){
break; break;
} }
#if GRIPPER_ENABLED == ENABLED #if AP_GRIPPER_ENABLED
if (failsafe_option(FailsafeOption::RELEASE_GRIPPER)) { if (failsafe_option(FailsafeOption::RELEASE_GRIPPER)) {
copter.g2.gripper.release(); copter.g2.gripper.release();
} }

View File

@ -1978,7 +1978,7 @@ bool ModeAuto::verify_payload_place()
case PayloadPlaceStateType_Releasing_Start: case PayloadPlaceStateType_Releasing_Start:
// Reinitialise vertical position controller to remove discontinuity due to touch down of payload // Reinitialise vertical position controller to remove discontinuity due to touch down of payload
pos_control->init_z_controller_no_descent(); pos_control->init_z_controller_no_descent();
#if GRIPPER_ENABLED == ENABLED #if AP_GRIPPER_ENABLED
if (g2.gripper.valid()) { if (g2.gripper.valid()) {
gcs().send_text(MAV_SEVERITY_INFO, "PayloadPlace: Releasing the gripper"); gcs().send_text(MAV_SEVERITY_INFO, "PayloadPlace: Releasing the gripper");
g2.gripper.release(); g2.gripper.release();
@ -1995,7 +1995,7 @@ bool ModeAuto::verify_payload_place()
nav_payload_place.state = PayloadPlaceStateType_Releasing; nav_payload_place.state = PayloadPlaceStateType_Releasing;
FALLTHROUGH; FALLTHROUGH;
case PayloadPlaceStateType_Releasing: case PayloadPlaceStateType_Releasing:
#if GRIPPER_ENABLED == ENABLED #if AP_GRIPPER_ENABLED
if (g2.gripper.valid() && !g2.gripper.released()) { if (g2.gripper.valid() && !g2.gripper.released()) {
return false; return false;
} }

View File

@ -27,7 +27,7 @@ void Copter::init_ardupilot()
#endif #endif
// init cargo gripper // init cargo gripper
#if GRIPPER_ENABLED == ENABLED #if AP_GRIPPER_ENABLED
g2.gripper.init(); g2.gripper.init();
#endif #endif