@@ -285,13 +285,9 @@ bool Copter::set_target_location(const Location& target_loc)
285285
286286 return mode_guided.set_destination (target_loc);
287287}
288- #endif // MODE_GUIDED_ENABLED
289- #endif // AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
290288
291- #if AP_SCRIPTING_ENABLED
292- #if MODE_GUIDED_ENABLED
293289// start takeoff to given altitude (for use by scripting)
294- bool Copter::start_takeoff (float alt)
290+ bool Copter::start_takeoff (const float alt)
295291{
296292 // exit if vehicle is not in Guided mode or Auto-Guided mode
297293 if (!flightmode->in_guided_mode ()) {
@@ -304,7 +300,11 @@ bool Copter::start_takeoff(float alt)
304300 }
305301 return false ;
306302}
303+ #endif // MODE_GUIDED_ENABLED
304+ #endif // AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
307305
306+ #if AP_SCRIPTING_ENABLED
307+ #if MODE_GUIDED_ENABLED
308308// set target position (for use by scripting)
309309bool Copter::set_target_pos_NED (const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt)
310310{
@@ -411,7 +411,55 @@ bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_
411411 mode_guided.set_angle (q, ang_vel_body, throttle, true );
412412 return true ;
413413}
414- #endif
414+
415+ // Register a custom mode with given number and names
416+ AP_Vehicle::custom_mode_state* Copter::register_custom_mode (const uint8_t num, const char * full_name, const char * short_name)
417+ {
418+ const Mode::Number number = (Mode::Number)num;
419+
420+ // See if this mode has been registered already, if it has return the state for it
421+ // This allows scripting restarts
422+ for (uint8_t i = 0 ; i < ARRAY_SIZE (mode_guided_custom); i++) {
423+ if (mode_guided_custom[i] == nullptr ) {
424+ break ;
425+ }
426+ if ((mode_guided_custom[i]->mode_number () == number) &&
427+ (strcmp (mode_guided_custom[i]->name (), full_name) == 0 ) &&
428+ (strncmp (mode_guided_custom[i]->name4 (), short_name, 4 ) == 0 )) {
429+ return &mode_guided_custom[i]->state ;
430+ }
431+ }
432+
433+ // Number already registered to existing mode
434+ if (mode_from_mode_num (number) != nullptr ) {
435+ return nullptr ;
436+ }
437+
438+ // Find free slot
439+ for (uint8_t i = 0 ; i < ARRAY_SIZE (mode_guided_custom); i++) {
440+ if (mode_guided_custom[i] == nullptr ) {
441+ // Duplicate strings so were not pointing to unknown memory
442+ const char * full_name_copy = strdup (full_name);
443+ const char * short_name_copy = strndup (short_name, 4 );
444+ if ((full_name_copy != nullptr ) && (short_name_copy != nullptr )) {
445+ mode_guided_custom[i] = NEW_NOTHROW ModeGuidedCustom (number, full_name_copy, short_name_copy);
446+ }
447+ if (mode_guided_custom[i] == nullptr ) {
448+ // Allocation failure
449+ return nullptr ;
450+ }
451+
452+ // Registration sucsessful, notify the GCS that it should re-request the avalable modes
453+ gcs ().available_modes_changed ();
454+
455+ return &mode_guided_custom[i]->state ;
456+ }
457+ }
458+
459+ // No free slots
460+ return nullptr ;
461+ }
462+ #endif // MODE_GUIDED_ENABLED
415463
416464#if MODE_CIRCLE_ENABLED
417465// circle mode controls
0 commit comments