AP_Scripting: applet for Plane follow on a switch

ArduPilot · May 22, 2024 · da71c7c · da71c7c
1 parent 8e399cf
commit da71c7c
Show file tree

Hide file tree

Showing 8 changed files with 438 additions and 0 deletions.
diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
@@ -963,6 +963,25 @@ bool Plane::flight_option_enabled(FlightOptions flight_option) const
  return g2.flight_options & flight_option;
 }
 
+// This will only apply in Guided mode, other modes ignore it
+// all the checks are in the Guided mode code, this is _desired_ speed
+bool Plane::set_desired_speed(float speed)
+{
+ plane.guided_state.target_airspeed_cm = speed * 100.0f;
+ plane.guided_state.target_airspeed_time_ms = AP_HAL::millis();
+
+ plane.calc_airspeed_errors();
+
+ return true;
+}
+
+// Helper function to let scripting set the guided mode radius
+bool Plane::set_guided_radius_and_direction(float radius, bool direction_is_ccw)
+{
+ plane.mode_guided.set_radius_and_direction(radius, direction_is_ccw);
+ return true;
+}
+
 #if AC_PRECLAND_ENABLED
 void Plane::precland_update(void)
 {

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
@@ -1045,6 +1045,8 @@ class Plane : public AP_Vehicle {
  void update_flight_stage();
  void set_flight_stage(AP_FixedWing::FlightStage fs);
  bool flight_option_enabled(FlightOptions flight_option) const;
+ bool set_desired_speed(float speed) override;
+ bool set_guided_radius_and_direction(float radius, bool direction_is_ccw) override;
 
  // navigation.cpp
  void loiter_angle_reset(void);

diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp
@@ -203,6 +203,7 @@ void Plane::calc_airspeed_errors()
  target_airspeed_cm = constrain_float(MAX(guided_state.target_airspeed_cm, target_airspeed_cm), aparm.airspeed_min *100, aparm.airspeed_max *100);
  }
 
+ //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "navigation guided - airspeed target cm: %d", target_airspeed_cm);
 #endif // OFFBOARD_GUIDED == ENABLED
 
 #if HAL_SOARING_ENABLED
@@ -270,6 +271,8 @@ void Plane::calc_airspeed_errors()
  // Apply airspeed limit
  target_airspeed_cm = constrain_int32(target_airspeed_cm, aparm.airspeed_min*100, aparm.airspeed_max*100);
 
+ //GCS_SEND_TEXT(MAV_SEVERITY_INFO, "navigation - airspeed target cm: %d", target_airspeed_cm);
+
  // use the TECS view of the target airspeed for reporting, to take
  // account of the landing speed
  airspeed_error = TECS_controller.get_target_airspeed() - airspeed_measured;

diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp
@@ -277,6 +277,7 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
  // exit immediately if not enabled
  if (!_enabled) {
+// printf("follow not enabled\n");
  return;
  }
 
@@ -285,6 +286,9 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
  return;
  }
 
+ //if (msg.sysid < 250)
+ // gcs().send_text(MAV_SEVERITY_INFO, "follow msg from: %d", msg.sysid);
+
  // skip message if not from our target
  if (_sysid != 0 && msg.sysid != _sysid) {
  if (_automatic_sysid) {
@@ -522,11 +526,13 @@ bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &ve
 bool AP_Follow::have_target(void) const
 {
  if (!_enabled) {
+// gcs().send_text(MAV_SEVERITY_INFO, "AP_Follow: not enabled");
  return false;
  }
 
  // check for timeout
  if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+// gcs().send_text(MAV_SEVERITY_INFO, "AP_Follow: timeout %d", _last_location_update_ms);
  return false;
  }
  return true;