Copter: return `custom_mode_state` for registered modes

This commit is contained in:
Iampete1 2024-11-14 15:40:52 +00:00 committed by Randy Mackay
parent fddfaaa1fc
commit 1ed57deab6
4 changed files with 40 additions and 12 deletions

View File

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

View File

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

View File

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

View File

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