Scripting: add access to AC_PID objects

AntennaTracker/Tracker.h

@@ -221,6 +221,7 @@ class Tracker : public AP_Vehicle {
     bool start_command_callback(const AP_Mission::Mission_Command& cmd) { return false; }
     void exit_mission_callback() { return; }
     bool verify_command_callback(const AP_Mission::Mission_Command& cmd) { return false; }
+    AC_PID *get_AC_PID(AC_PID_TYPE type) override;
 
     // tracking.cpp
     void update_vehicle_pos_estimate();

AntennaTracker/system.cpp

@@ -239,6 +239,27 @@ bool Tracker::should_log(uint32_t mask)
     return true;
 }
 
+AC_PID* Tracker::get_AC_PID(AC_PID_TYPE type)
+{
+    AC_PID *ret = nullptr;
+
+    switch(type) {
+        case AC_PID_TYPE::TRACKER_PITCH:
+            ret = &g.pidPitch2Srv;
+            break;
+
+        case AC_PID_TYPE::TRACKER_YAW:
+            ret = &g.pidYaw2Srv;
+            break;
+
+        default:
+            break;
+    }
+
+    return ret;
+}
+
+
 
 #include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
 #include <AP_Avoidance/AP_Avoidance.h>

ArduCopter/AP_Vehicle.cpp

@@ -0,0 +1,78 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include "Copter.h"
+
+// start takeoff to given altitude (for use by scripting)
+bool Copter::start_takeoff(float alt)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!flightmode->in_guided_mode()) {
+        return false;
+    }
+
+    if (mode_guided.do_user_takeoff_start(alt * 100.0f)) {
+        copter.set_auto_armed(true);
+        return true;
+    }
+    return false;
+}
+
+// set target location (for use by scripting)
+bool Copter::set_target_location(const Location& target_loc)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!flightmode->in_guided_mode()) {
+        return false;
+    }
+
+    return mode_guided.set_destination(target_loc);
+}
+
+bool Copter::set_target_velocity_NED(const Vector3f& vel_ned)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!flightmode->in_guided_mode()) {
+        return false;
+    }
+
+    // convert vector to neu in cm
+    const Vector3f vel_neu_cms(vel_ned.x * 100.0f, vel_ned.y * 100.0f, -vel_ned.z * 100.0f);
+    mode_guided.set_velocity(vel_neu_cms);
+    return true;
+}
+
+AC_PID* Copter::get_AC_PID(AC_PID_TYPE type)
+{
+    AC_PID *ret = nullptr;
+
+    switch(type) {
+        case AC_PID_TYPE::RATE_ROLL:
+            ret = &copter.attitude_control->get_rate_roll_pid();
+            break;
+
+        case AC_PID_TYPE::RATE_PITCH:
+            ret = &copter.attitude_control->get_rate_pitch_pid();
+            break;
+
+        case AC_PID_TYPE::RATE_YAW:
+            ret = &copter.attitude_control->get_rate_yaw_pid();
+            break;
+
+        default:
+            break;
+    }
+
+    return ret;
+}

ArduCopter/Copter.cpp

@@ -270,45 +270,6 @@ void Copter::fast_loop()
     AP_Vehicle::fast_loop();
 }
 
-// start takeoff to given altitude (for use by scripting)
-bool Copter::start_takeoff(float alt)
-{
-    // exit if vehicle is not in Guided mode or Auto-Guided mode
-    if (!flightmode->in_guided_mode()) {
-        return false;
-    }
-
-    if (mode_guided.do_user_takeoff_start(alt * 100.0f)) {
-        copter.set_auto_armed(true);
-        return true;
-    }
-    return false;
-}
-
-// set target location (for use by scripting)
-bool Copter::set_target_location(const Location& target_loc)
-{
-    // exit if vehicle is not in Guided mode or Auto-Guided mode
-    if (!flightmode->in_guided_mode()) {
-        return false;
-    }
-
-    return mode_guided.set_destination(target_loc);
-}
-
-bool Copter::set_target_velocity_NED(const Vector3f& vel_ned)
-{
-    // exit if vehicle is not in Guided mode or Auto-Guided mode
-    if (!flightmode->in_guided_mode()) {
-        return false;
-    }
-
-    // convert vector to neu in cm
-    const Vector3f vel_neu_cms(vel_ned.x * 100.0f, vel_ned.y * 100.0f, -vel_ned.z * 100.0f);
-    mode_guided.set_velocity(vel_neu_cms);
-    return true;
-}
-
 // rc_loops - reads user input from transmitter/receiver
 // called at 100hz
 void Copter::rc_loop()

ArduCopter/Copter.h

@@ -626,14 +626,17 @@ class Copter : public AP_Vehicle {
     void set_failsafe_gcs(bool b);
     void update_using_interlock();
 
+    //AP_Vehicle.cpp
+    bool start_takeoff(float alt) override;
+    bool set_target_location(const Location& target_loc) override;
+    bool set_target_velocity_NED(const Vector3f& vel_ned) override;
+    AC_PID *get_AC_PID(AC_PID_TYPE type) override;
+
     // Copter.cpp
     void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
                              uint8_t &task_count,
                              uint32_t &log_bit) override;
     void fast_loop() override;
-    bool start_takeoff(float alt) override;
-    bool set_target_location(const Location& target_loc) override;
-    bool set_target_velocity_NED(const Vector3f& vel_ned) override;
     void rc_loop();
     void throttle_loop();
     void update_batt_compass(void);

ArduCopter/tuning.cpp

@@ -39,18 +39,18 @@ void Copter::tuning()
         break;
 
     case TUNING_RATE_ROLL_PITCH_KP:
-        attitude_control->get_rate_roll_pid().kP(tuning_value);
-        attitude_control->get_rate_pitch_pid().kP(tuning_value);
+        attitude_control->get_rate_roll_pid().set_kP(tuning_value);
+        attitude_control->get_rate_pitch_pid().set_kP(tuning_value);
         break;
 
     case TUNING_RATE_ROLL_PITCH_KI:
-        attitude_control->get_rate_roll_pid().kI(tuning_value);
-        attitude_control->get_rate_pitch_pid().kI(tuning_value);
+        attitude_control->get_rate_roll_pid().set_kI(tuning_value);
+        attitude_control->get_rate_pitch_pid().set_kI(tuning_value);
         break;
 
     case TUNING_RATE_ROLL_PITCH_KD:
-        attitude_control->get_rate_roll_pid().kD(tuning_value);
-        attitude_control->get_rate_pitch_pid().kD(tuning_value);
+        attitude_control->get_rate_roll_pid().set_kD(tuning_value);
+        attitude_control->get_rate_pitch_pid().set_kD(tuning_value);
         break;
 
     // Yaw tuning
@@ -59,11 +59,11 @@ void Copter::tuning()
         break;
 
     case TUNING_YAW_RATE_KP:
-        attitude_control->get_rate_yaw_pid().kP(tuning_value);
+        attitude_control->get_rate_yaw_pid().set_kP(tuning_value);
         break;
 
     case TUNING_YAW_RATE_KD:
-        attitude_control->get_rate_yaw_pid().kD(tuning_value);
+        attitude_control->get_rate_yaw_pid().set_kD(tuning_value);
         break;
 
     // Altitude and throttle tuning
@@ -76,15 +76,15 @@ void Copter::tuning()
         break;
 
     case TUNING_ACCEL_Z_KP:
-        pos_control->get_accel_z_pid().kP(tuning_value);
+        pos_control->get_accel_z_pid().set_kP(tuning_value);
         break;
 
     case TUNING_ACCEL_Z_KI:
-        pos_control->get_accel_z_pid().kI(tuning_value);
+        pos_control->get_accel_z_pid().set_kI(tuning_value);
         break;
 
     case TUNING_ACCEL_Z_KD:
-        pos_control->get_accel_z_pid().kD(tuning_value);
+        pos_control->get_accel_z_pid().set_kD(tuning_value);
         break;
 
     // Loiter and navigation tuning
@@ -120,15 +120,15 @@ void Copter::tuning()
         break;
 
     case TUNING_RATE_PITCH_FF:
-        attitude_control->get_rate_pitch_pid().ff(tuning_value);
+        attitude_control->get_rate_pitch_pid().set_ff(tuning_value);
         break;
 
     case TUNING_RATE_ROLL_FF:
-        attitude_control->get_rate_roll_pid().ff(tuning_value);
+        attitude_control->get_rate_roll_pid().set_ff(tuning_value);
         break;
 
     case TUNING_RATE_YAW_FF:
-        attitude_control->get_rate_yaw_pid().ff(tuning_value);
+        attitude_control->get_rate_yaw_pid().set_ff(tuning_value);
         break;
 #endif
 
@@ -154,27 +154,27 @@ void Copter::tuning()
         break;
 
     case TUNING_RATE_PITCH_KP:
-        attitude_control->get_rate_pitch_pid().kP(tuning_value);
+        attitude_control->get_rate_pitch_pid().set_kP(tuning_value);
         break;
 
     case TUNING_RATE_PITCH_KI:
-        attitude_control->get_rate_pitch_pid().kI(tuning_value);
+        attitude_control->get_rate_pitch_pid().set_kI(tuning_value);
         break;
 
     case TUNING_RATE_PITCH_KD:
-        attitude_control->get_rate_pitch_pid().kD(tuning_value);
+        attitude_control->get_rate_pitch_pid().set_kD(tuning_value);
         break;
 
     case TUNING_RATE_ROLL_KP:
-        attitude_control->get_rate_roll_pid().kP(tuning_value);
+        attitude_control->get_rate_roll_pid().set_kP(tuning_value);
         break;
 
     case TUNING_RATE_ROLL_KI:
-        attitude_control->get_rate_roll_pid().kI(tuning_value);
+        attitude_control->get_rate_roll_pid().set_kI(tuning_value);
         break;
 
     case TUNING_RATE_ROLL_KD:
-        attitude_control->get_rate_roll_pid().kD(tuning_value);
+        attitude_control->get_rate_roll_pid().set_kD(tuning_value);
         break;
 
 #if FRAME_CONFIG != HELI_FRAME
@@ -184,7 +184,7 @@ void Copter::tuning()
 #endif
 
      case TUNING_RATE_YAW_FILT:
-         attitude_control->get_rate_yaw_pid().filt_E_hz(tuning_value);
+         attitude_control->get_rate_yaw_pid().set_filt_E_hz(tuning_value);
          break;
 
 #if WINCH_ENABLED == ENABLED

ArduPlane/AP_Vehicle.cpp

@@ -0,0 +1,79 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include "Plane.h"
+
+// set target location (for use by scripting)
+bool Plane::set_target_location(const Location& target_loc)
+{
+    if (plane.control_mode != &plane.mode_guided) {
+        // only accept position updates when in GUIDED mode
+        return false;
+    }
+    plane.guided_WP_loc = target_loc;
+    // add home alt if needed
+    if (plane.guided_WP_loc.relative_alt) {
+        plane.guided_WP_loc.alt += plane.home.alt;
+        plane.guided_WP_loc.relative_alt = 0;
+    }
+    plane.set_guided_WP();
+    return true;
+}
+
+// set target location (for use by scripting)
+bool Plane::get_target_location(Location& target_loc)
+{
+    switch (control_mode->mode_number()) {
+    case Mode::Number::RTL:
+    case Mode::Number::AVOID_ADSB:
+    case Mode::Number::GUIDED:
+    case Mode::Number::AUTO:
+    case Mode::Number::LOITER:
+    case Mode::Number::QLOITER:
+    case Mode::Number::QLAND:
+    case Mode::Number::QRTL:
+        target_loc = next_WP_loc;
+        return true;
+        break;
+    default:
+        break;
+    }
+    return false;
+}
+
+AC_PID* Plane::get_AC_PID(AC_PID_TYPE type)
+{
+    AC_PID *ret = nullptr;
+
+    if (plane.quadplane.enabled()) {
+        switch(type) {
+            case AC_PID_TYPE::RATE_ROLL:
+                ret = &quadplane.attitude_control->get_rate_roll_pid();
+                break;
+
+            case AC_PID_TYPE::RATE_PITCH:
+                ret = &quadplane.attitude_control->get_rate_pitch_pid();
+                break;
+
+            case AC_PID_TYPE::RATE_YAW:
+                ret = &quadplane.attitude_control->get_rate_yaw_pid();
+                break;
+
+            default:
+                break;
+        }
+    }
+
+    return ret;
+}