Skip to content

Commit

Permalink
add parachute-unavailable code
Browse files Browse the repository at this point in the history
  • Loading branch information
peterbarker committed Feb 2, 2024
1 parent cd1e127 commit fcbdbab
Show file tree
Hide file tree
Showing 7 changed files with 106 additions and 19 deletions.
2 changes: 1 addition & 1 deletion ArduCopter/Copter.h
Original file line number Diff line number Diff line change
Expand Up @@ -528,7 +528,7 @@ class Copter : public AP_Vehicle {
#endif

// Parachute release
#if PARACHUTE == ENABLED
#if PARACHUTE == ENABLED || AP_PARACHUTE_UNAVAILABLE_ENABLED
AP_Parachute parachute;
#endif

Expand Down
4 changes: 4 additions & 0 deletions ArduCopter/land_detector.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,10 @@ void Copter::update_land_and_crash_detectors()
#if PARACHUTE == ENABLED
// check parachute
parachute_check();
#elif AP_PARACHUTE_UNAVAILABLE_ENABLED
if (AP::parachute()->mistakenly_enabled()) {
AP_BoardConfig::config_error("Parachute enabled but not compiled in");
}
#endif

crash_check();
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/Plane.h
Original file line number Diff line number Diff line change
Expand Up @@ -634,7 +634,7 @@ class Plane : public AP_Vehicle {
FUNCTOR_BIND_MEMBER(&Plane::exit_mission_callback, void)};


#if PARACHUTE == ENABLED
#if PARACHUTE == ENABLED || AP_PARACHUTE_UNAVAILABLE_ENABLED
AP_Parachute parachute;
#endif

Expand Down
4 changes: 4 additions & 0 deletions ArduPlane/parachute.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,10 @@ void Plane::parachute_check()
#if PARACHUTE == ENABLED
parachute.update();
parachute.check_sink_rate();
#elif AP_PARACHUTE_UNAVAILABLE_ENABLED
if (AP::parachute()->mistakenly_enabled()) {
AP_BoardConfig::config_error("Parachute enabled but not compiled in");
}
#endif
}

Expand Down
50 changes: 40 additions & 10 deletions libraries/AP_Parachute/AP_Parachute.cpp
Original file line number Diff line number Diff line change
@@ -1,15 +1,25 @@
#include "AP_Parachute_config.h"

#if HAL_PARACHUTE_ENABLED || AP_PARACHUTE_UNAVAILABLE_ENABLED

#if HAL_PARACHUTE_ENABLED && AP_PARACHUTE_UNAVAILABLE_ENABLED
#error "Only one of HAL_PARACHUTE_ENABLED and AP_PARACHUTE_UNAVAILABLE_ENABLED can be set"
#endif

#include "AP_Parachute.h"

#if HAL_PARACHUTE_ENABLED
/*
* This is not standard parameter conversion code; it is
* compatability code we are using to remove parachute support from
* 1MB boards without leaving users that *are* using parachutes on
* 1MB boards with a sneakily broken parachute system. We enable
* just enough of the parachute library to check if the enable
* parameter is set; if it is we throw a config error, making the
* vehicle non-flyable.
*/
// PARAMETER_CONVERSION - Added: Feb-2024

#include <AP_Relay/AP_Relay.h>
#include <AP_Math/AP_Math.h>
#include <RC_Channel/RC_Channel.h>
#include <SRV_Channel/SRV_Channel.h>
#include <AP_Notify/AP_Notify.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Logger/AP_Logger.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Param/AP_Param.h>

extern const AP_HAL::HAL& hal;

Expand All @@ -22,6 +32,7 @@ const AP_Param::GroupInfo AP_Parachute::var_info[] = {
// @User: Standard
AP_GROUPINFO_FLAGS("ENABLED", 0, AP_Parachute, _enabled, 0, AP_PARAM_FLAG_ENABLE),

#if HAL_PARACHUTE_ENABLED
// @Param: TYPE
// @DisplayName: Parachute release mechanism type (relay or servo)
// @Description: Parachute release mechanism type (relay number in versions prior to 4.5, or servo). Values 0-3 all are relay. Relay number used for release is set by RELAYx_FUNCTION in 4.5 or later.
Expand Down Expand Up @@ -81,10 +92,24 @@ const AP_Param::GroupInfo AP_Parachute::var_info[] = {
// @Bitmask: 0:hold open forever after release
// @User: Standard
AP_GROUPINFO("OPTIONS", 7, AP_Parachute, _options, 0),
#endif // HAL_PARACHUTE_ENABLED

AP_GROUPEND
};

#endif // HAL_PARACHUTE_ENABLED || AP_PARACHUTE_UNAVAILABLE_ENABLED

#if HAL_PARACHUTE_ENABLED

#include <AP_Relay/AP_Relay.h>
#include <AP_Math/AP_Math.h>
#include <RC_Channel/RC_Channel.h>
#include <SRV_Channel/SRV_Channel.h>
#include <AP_Notify/AP_Notify.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Logger/AP_Logger.h>
#include <GCS_MAVLink/GCS.h>

/// enabled - enable or disable parachute release
void AP_Parachute::enabled(bool on_off)
{
Expand Down Expand Up @@ -249,6 +274,10 @@ bool AP_Parachute::get_legacy_relay_index(int8_t &index) const
}
#endif

#endif // HAL_PARACHUTE_ENABLED

#if HAL_PARACHUTE_ENABLED || AP_PARACHUTE_UNAVAILABLE_ENABLED

// singleton instance
AP_Parachute *AP_Parachute::_singleton;

Expand All @@ -260,4 +289,5 @@ AP_Parachute *parachute()
}

}
#endif // HAL_PARACHUTE_ENABLED

#endif // HAL_PARACHUTE_ENABLED || AP_PARACHUTE_UNAVAILABLE_ENABLED
51 changes: 44 additions & 7 deletions libraries/AP_Parachute/AP_Parachute.h
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,10 @@
/// @brief Parachute release library
#pragma once

#include "AP_Parachute_config.h"

#if HAL_PARACHUTE_ENABLED

#include <AP_Param/AP_Param.h>
#include <AP_Common/AP_Common.h>

Expand All @@ -21,13 +25,6 @@

#define AP_PARACHUTE_CRITICAL_SINK_DEFAULT 0 // default critical sink speed in m/s to trigger emergency parachute

#ifndef HAL_PARACHUTE_ENABLED
// default to parachute enabled to match previous configs
#define HAL_PARACHUTE_ENABLED 1
#endif

#if HAL_PARACHUTE_ENABLED

/// @class AP_Parachute
/// @brief Class managing the release of a parachute
class AP_Parachute {
Expand Down Expand Up @@ -124,4 +121,44 @@ namespace AP {
AP_Parachute *parachute();
};

#elif AP_PARACHUTE_UNAVAILABLE_ENABLED

/*
* minimal class definition for a shim AP_Parachute. just enough to be
* able to interpret the "enabled" parameter.
*/

#include <AP_Param/AP_Param.h>
class AP_Parachute {
public:
/// Constructor
AP_Parachute()
{
_singleton = this;
AP_Param::setup_object_defaults(this, var_info);

AP_Param::setup_object_defaults(this, var_info);
}

/* Do not allow copies */
CLASS_NO_COPY(AP_Parachute);

AP_Int8 _enabled; // 1 if parachute release is enabled

static const struct AP_Param::GroupInfo var_info[];

/// returns true if parachute is enabled but compiled out of the code
bool mistakenly_enabled() const { return _enabled != 0; }

// get singleton instance
static AP_Parachute *get_singleton() { return _singleton; }

private:
static AP_Parachute *_singleton;
};

namespace AP {
AP_Parachute *parachute();
};

#endif // HAL_PARACHUTE_ENABLED
12 changes: 12 additions & 0 deletions libraries/AP_Parachute/AP_Parachute_config.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
#pragma once

#include <AP_HAL/AP_HAL_Boards.h>

#ifndef HAL_PARACHUTE_ENABLED
// default to parachute enabled to match previous configs
#define HAL_PARACHUTE_ENABLED 1
#endif

#ifndef AP_PARACHUTE_UNAVAILABLE_ENABLED
#define AP_PARACHUTE_UNAVAILABLE_ENABLED !HAL_PARACHUTE_ENABLED && BOARD_FLASH_SIZE <= 1024
#endif

0 comments on commit fcbdbab

Please sign in to comment.