호버링 코드

Individual_moter_control_code.py

@@ -0,0 +1,177 @@
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/tasks.h>
+#include <px4_platform_common/time.h>
+
+#include <lib/module_params/module_params.h>
+#include <parameters/param.h>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/actuator_motors.h>
+#include <uORB/topics/actuator_outputs.h>
+
+using namespace time_literals;
+
+class MotorGainPostMixer : public ModuleBase<MotorGainPostMixer>, public ModuleParams
+{
+public:
+    MotorGainPostMixer() :
+        ModuleParams(nullptr),
+        _sub_motors(ORB_ID(actuator_motors)),
+        _pub_outputs(ORB_ID(actuator_outputs))
+    {
+    }
+
+    ~MotorGainPostMixer() override {}
+
+    static int task_spawn(int argc, char *argv[])
+    {
+        MotorGainPostMixer *instance = new MotorGainPostMixer();
+        if (!instance) {
+            PX4_ERR("alloc failed");
+            return PX4_ERROR;
+        }
+
+        _object.store(instance);
+
+        _task_id = px4_task_spawn_cmd("motor_gain_post_mixer",
+                                      SCHED_DEFAULT,
+                                      SCHED_PRIORITY_DEFAULT,
+                                      1700,
+                                      (px4_main_t)&MotorGainPostMixer::task_trampoline,
+                                      nullptr);
+
+        if (_task_id < 0) {
+            PX4_ERR("task spawn failed");
+            delete instance;
+            return PX4_ERROR;
+        }
+
+        return PX4_OK;
+    }
+
+    static MotorGainPostMixer *instantiate(int argc, char *argv[]) { return new MotorGainPostMixer(); }
+    static int custom_command(int argc, char *argv[]) { return print_usage("Unknown command"); }
+    static int print_usage(const char *reason);
+
+    void run() override;
+
+private:
+
+    DEFINE_PARAMETERS(
+        (ParamInt<px4::params::MGC_EN>) _enabled,
+        (ParamFloat<px4::params::MGC_G1>) _g1,
+        (ParamFloat<px4::params::MGC_G2>) _g2,
+        (ParamFloat<px4::params::MGC_G3>) _g3,
+        (ParamFloat<px4::params::MGC_G4>) _g4
+    );
+
+    uORB::Subscription _sub_motors;
+    uORB::Publication<actuator_outputs_s> _pub_outputs;
+
+    static MotorGainPostMixer *_object;
+    static int _task_id;
+};
+
+MotorGainPostMixer *MotorGainPostMixer::_object = nullptr;
+int MotorGainPostMixer::_task_id = -1;
+
+void MotorGainPostMixer::run()
+{
+    PX4_INFO("motor_gain_post_mixer started");
+
+    actuator_motors_s motors{};
+    actuator_outputs_s outputs{};
+
+    // safety: initialize outputs to zero
+    outputs.noutputs = 0;
+    for (unsigned i = 0; i < sizeof(outputs.output)/sizeof(outputs.output[0]); i++) {
+        outputs.output[i] = 0.0f;
+    }
+
+    // loop rate ~ 250-400 Hz depending on source; we'll poll with timeout
+    const int wait_ms = 4; // ~250 Hz
+
+    while (!should_exit()) {
+
+        // if module disabled, just sleep and continue
+        if (!_enabled.get()) {
+            px4_usleep(wait_ms * 1000);
+            // optionally we could still swallow the topic to keep it fresh
+            (void)_sub_motors.update(&motors);
+            continue;
+        }
+
+        // wait for actuator_motors update
+        if (_sub_motors.updated()) {
+            if (_sub_motors.copy(&motors) != PX4_OK) {
+                px4_usleep(wait_ms * 1000);
+                continue;
+            }
+
+            // Prepare outputs structure based on motors
+            outputs.timestamp = hrt_absolute_time();
+
+            // motors.noutputs indicates number of motor channels produced by mixer
+            unsigned n = motors.noutputs;
+            if (n > sizeof(outputs.output)/sizeof(outputs.output[0])) {
+                // clamp for safety
+                n = sizeof(outputs.output)/sizeof(outputs.output[0]);
+            }
+
+            outputs.noutputs = n;
+
+            // Gains from params (default 1.0)
+            float gains[4] = { _g1.get(), _g2.get(), _g3.get(), _g4.get() };
+
+            // Apply per-motor gains for available channels.
+            // motors.control[] holds the post-mixer normalized outputs (0..1 or -1..1 depending on reversible flags).
+            for (unsigned i = 0; i < n; ++i) {
+                float base = motors.control[i];
+
+                // if we only specified 4 gains but more channels exist, apply 1.0 for extra channels
+                float g = (i < 4) ? gains[i] : 1.0f;
+
+                // apply gain
+                float scaled = base * g;
+
+                // safety clamp to reasonable range: [-1.0, 1.0] (actuator_outputs expects normalized outputs)
+                if (scaled > 1.0f) scaled = 1.0f;
+                if (scaled < -1.0f) scaled = -1.0f;
+
+                outputs.output[i] = scaled;
+            }
+
+            // publish actuator_outputs (this will be used by IO/PWM driver)
+            if (!_pub_outputs.advertised()) {
+                _pub_outputs.advertise(outputs);
+            } else {
+                _pub_outputs.publish(outputs);
+            }
+        }
+
+        px4_usleep(wait_ms * 1000);
+    }
+
+    PX4_INFO("motor_gain_post_mixer exiting");
+}
+
+int MotorGainPostMixer::print_usage(const char *reason)
+{
+    if (reason) {
+        PX4_WARN("%s", reason);
+    }
+
+    PRINT_MODULE_USAGE_NAME("motor_gain_post_mixer", "module");
+    PRINT_MODULE_USAGE_COMMAND("start");
+    PRINT_MODULE_USAGE_PARAM_INT('h', 0, 0, "unused", true);
+    PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+    return 0;
+}
+
+extern "C" __EXPORT int motor_gain_post_mixer_main(int argc, char *argv[])
+{
+    return MotorGainPostMixer::main(argc, argv);
+}

failsafe_code.py

@@ -0,0 +1,184 @@
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_command.h>
+
+#include <math.h>
+
+class CustomFailsafe : public ModuleBase<CustomFailsafe>
+{
+public:
+    CustomFailsafe() = default;
+    ~CustomFailsafe() = default;
+
+    static int task_spawn(int argc, char *argv[]);
+    static CustomFailsafe *instantiate(int argc, char *argv[]);
+    static int custom_command(int argc, char *argv[]);
+    static int print_usage(const char *reason = nullptr);
+
+    void run() override;
+    void stop() { _running = false; }
+
+private:
+
+    bool _running{true};
+
+    // 비행 금지 구역 설정
+    const float nofly_lat = 37.123456;
+    const float nofly_lon = 127.123456;
+    const float nofly_radius = 100.0f;
+
+    const float failsafe_hover_alt = 5.0f;   // 5m 위로 상승
+
+    // 구독
+    uORB::Subscription _sub_gpos{ORB_ID(vehicle_global_position)};
+    uORB::Subscription _sub_lpos{ORB_ID(vehicle_local_position)};
+    uORB::Subscription _sub_status{ORB_ID(vehicle_status)};
+    uORB::Subscription _sub_telem{ORB_ID(telemetry_status)};
+    uORB::Subscription _sub_batt{ORB_ID(battery_status)};
+
+    // 명령 발행
+    uORB::Publication<vehicle_command_s> _pub_cmd{ORB_ID(vehicle_command)};
+
+    // 편의 함수들
+    float calc_distance_m(float lat1, float lon1, float lat2, float lon2);
+    void send_hover_command(float rel_alt);
+    void send_RTL();
+    void send_land();
+};
+
+float CustomFailsafe::calc_distance_m(float lat1, float lon1, float lat2, float lon2)
+{
+    constexpr float R = 6371000.0f;
+
+    float dlat = (lat2 - lat1) * M_PI / 180.0f;
+    float dlon = (lon2 - lon1) * M_PI / 180.0f;
+
+    float a = sinf(dlat/2)*sinf(dlat/2) +
+              cosf(lat1*M_PI/180.0f)*cosf(lat2*M_PI/180.0f) *
+              sinf(dlon/2)*sinf(dlon/2);
+
+    float c = 2 * atan2f(sqrtf(a), sqrtf(1-a));
+    return R * c;
+}
+
+void CustomFailsafe::send_hover_command(float rel_alt)
+{
+    vehicle_command_s cmd{};
+    cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LOITER;
+    cmd.param7 = rel_alt;
+    _pub_cmd.publish(cmd);
+    PX4_WARN("Failsafe: Hover at %.2f m", (double)rel_alt);
+}
+
+void CustomFailsafe::send_RTL()
+{
+    vehicle_command_s cmd{};
+    cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH;
+    _pub_cmd.publish(cmd);
+    PX4_WARN("Failsafe: RTL triggered!");
+}
+
+void CustomFailsafe::send_land()
+{
+    vehicle_command_s cmd{};
+    cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
+    _pub_cmd.publish(cmd);
+    PX4_WARN("Failsafe: Critical battery → Landing now!");
+}
+
+int CustomFailsafe::task_spawn(int argc, char *argv[])
+{
+    _task_id = px4_task_spawn_cmd("custom_failsafe",
+                                  SCHED_DEFAULT,
+                                  SCHED_PRIORITY_DEFAULT,
+                                  3000,
+                                  (px4_main_t)&run_trampoline,
+                                  nullptr);
+    return _task_id >= 0 ? 0 : -1;
+}
+
+CustomFailsafe *CustomFailsafe::instantiate(int argc, char *argv[])
+{
+    return new CustomFailsafe();
+}
+
+int CustomFailsafe::custom_command(int argc, char *argv[]) { return print_usage(); }
+int CustomFailsafe::print_usage(const char *reason) { PX4_INFO("Usage: custom_failsafe start"); return 0; }
+
+
+void CustomFailsafe::run()
+{
+    PX4_INFO("Custom Failsafe module running...");
+
+    while (_running) {
+
+        // 1) 위치 정보
+        vehicle_global_position_s gpos{};
+        vehicle_local_position_s lpos{};
+        vehicle_status_s status{};
+        telemetry_status_s telem{};
+        battery_status_s batt{};
+
+        _sub_gpos.update(&gpos);
+        _sub_lpos.update(&lpos);
+        _sub_status.update(&status);
+        _sub_telem.update(&telem);
+        _sub_batt.update(&batt);
+
+        // --------------------------
+        // A) GPS 오류 (Position Loss)
+        // --------------------------
+        if (!gpos.valid || gpos.eph > 3.0f || gpos.epv > 3.0f) {
+            PX4_WARN("Failsafe: GPS Lost → Hovering");
+            send_hover_command( failsafe_hover_alt );
+            px4_usleep(200000);
+            continue;
+        }
+
+        // --------------------------
+        // B) 통신 두절 (RC/Telemetry Loss)
+        // --------------------------
+        if (status.rc_signal_lost || telem.type == telemetry_status_s::LINK_TYPE_UNKNOWN) {
+            PX4_WARN("Failsafe: RC/Telemetry Lost → RTL");
+            send_RTL();
+            px4_usleep(200000);
+            continue;
+        }
+
+        // --------------------------
+        // C) 배터리 부족
+        // --------------------------
+        if (batt.warning == battery_status_s::BATTERY_WARNING_LOW) {
+            PX4_WARN("Battery Warning: Consider RTL soon");
+        }
+
+        if (batt.warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
+            send_land();
+            px4_usleep(200000);
+            continue;
+        }
+
+        // --------------------------
+        // D) 금지구역 진입 감지
+        // --------------------------
+        float dist = calc_distance_m(gpos.lat, gpos.lon, nofly_lat, nofly_lon);
+
+        if (dist < nofly_radius) {
+            PX4_WARN("Entered NO-FLY ZONE → Hover fail-safe");
+            send_hover_command(failsafe_hover_alt);
+            px4_usleep(200000);
+            continue;
+        }
+
+        px4_usleep(20000);
+    }
+}
+