diff --git a/Individual_moter_control_code.py b/Individual_moter_control_code.py new file mode 100644 index 0000000..477105d --- /dev/null +++ b/Individual_moter_control_code.py @@ -0,0 +1,177 @@ +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +using namespace time_literals; + +class MotorGainPostMixer : public ModuleBase, 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) _enabled, + (ParamFloat) _g1, + (ParamFloat) _g2, + (ParamFloat) _g3, + (ParamFloat) _g4 + ); + + uORB::Subscription _sub_motors; + uORB::Publication _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); +} diff --git a/README.md b/README.md deleted file mode 100644 index 8019425..0000000 --- a/README.md +++ /dev/null @@ -1 +0,0 @@ -# aeroewha diff --git a/failsafe_code.py b/failsafe_code.py new file mode 100644 index 0000000..83aa4b0 --- /dev/null +++ b/failsafe_code.py @@ -0,0 +1,184 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +class CustomFailsafe : public ModuleBase +{ +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 _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); + } +} +