mirror of https://github.com/ArduPilot/ardupilot
Copter: return `custom_mode_state` for registered modes
This commit is contained in:
parent
fddfaaa1fc
commit
1ed57deab6
|
@ -413,18 +413,26 @@ bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_
|
|||
}
|
||||
|
||||
// Register a custom mode with given number and names
|
||||
bool Copter::register_custom_mode(const uint8_t num, const char* full_name, const char* short_name)
|
||||
AP_Vehicle::custom_mode_state* Copter::register_custom_mode(const uint8_t num, const char* full_name, const char* short_name)
|
||||
{
|
||||
// Numbers over 100 are reserved for custom modes
|
||||
if (num < 100) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const Mode::Number number = (Mode::Number)num;
|
||||
|
||||
// See if this mode has been registered already, if it has return the state for it
|
||||
// This allows scripting restarts
|
||||
for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
|
||||
if (mode_guided_custom[i] == nullptr) {
|
||||
break;
|
||||
}
|
||||
if ((mode_guided_custom[i]->mode_number() == number) &&
|
||||
(strcmp(mode_guided_custom[i]->name(), full_name) == 0) &&
|
||||
(strncmp(mode_guided_custom[i]->name4(), short_name, 4) == 0)) {
|
||||
return &mode_guided_custom[i]->state;
|
||||
}
|
||||
}
|
||||
|
||||
// Number already registered to existing mode
|
||||
if (mode_from_mode_num(number) != nullptr) {
|
||||
return false;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
// Find free slot
|
||||
|
@ -436,12 +444,16 @@ bool Copter::register_custom_mode(const uint8_t num, const char* full_name, cons
|
|||
if ((full_name_copy != nullptr) && (short_name_copy != nullptr)) {
|
||||
mode_guided_custom[i] = NEW_NOTHROW ModeGuidedCustom(number, full_name_copy, short_name_copy);
|
||||
}
|
||||
return mode_guided_custom[i] != nullptr;
|
||||
if (mode_guided_custom[i] == nullptr) {
|
||||
// Allocation failure
|
||||
return nullptr;
|
||||
}
|
||||
return &mode_guided_custom[i]->state;
|
||||
}
|
||||
}
|
||||
|
||||
// No free slots
|
||||
return false;
|
||||
return nullptr;
|
||||
}
|
||||
#endif // MODE_GUIDED_ENABLED
|
||||
|
||||
|
|
|
@ -685,7 +685,7 @@ private:
|
|||
bool set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle) override;
|
||||
|
||||
// Register a custom mode with given number and names
|
||||
bool register_custom_mode(const uint8_t number, const char* full_name, const char* short_name) override;
|
||||
AP_Vehicle::custom_mode_state* register_custom_mode(const uint8_t number, const char* full_name, const char* short_name) override;
|
||||
#endif
|
||||
#if MODE_CIRCLE_ENABLED
|
||||
bool get_circle_radius(float &radius_m) override;
|
||||
|
@ -1033,7 +1033,7 @@ private:
|
|||
ModeGuided mode_guided;
|
||||
#if AP_SCRIPTING_ENABLED
|
||||
// Custom modes registered at runtime
|
||||
Mode *mode_guided_custom[5];
|
||||
ModeGuidedCustom *mode_guided_custom[5];
|
||||
#endif
|
||||
#endif
|
||||
ModeLand mode_land;
|
||||
|
|
|
@ -1208,12 +1208,16 @@ public:
|
|||
// constructor registers custom number and names
|
||||
ModeGuidedCustom(const Number _number, const char* _full_name, const char* _short_name);
|
||||
|
||||
bool init(bool ignore_checks) override;
|
||||
|
||||
Number mode_number() const override { return number; }
|
||||
|
||||
protected:
|
||||
const char *name() const override { return full_name; }
|
||||
const char *name4() const override { return short_name; }
|
||||
|
||||
// State object which can be edited by scripting
|
||||
AP_Vehicle::custom_mode_state state;
|
||||
|
||||
private:
|
||||
const Number number;
|
||||
const char* full_name;
|
||||
|
|
|
@ -8,4 +8,16 @@ ModeGuidedCustom::ModeGuidedCustom(const Number _number, const char* _full_name,
|
|||
short_name(_short_name)
|
||||
{
|
||||
}
|
||||
|
||||
bool ModeGuidedCustom::init(bool ignore_checks)
|
||||
{
|
||||
// Stript can block entry
|
||||
if (!state.allow_entry) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Guided entry checks must also pass
|
||||
return ModeGuided::init(ignore_checks);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
Loading…
Reference in New Issue