From f6aaa4e84e36877a9984630ff56cbadd177df509 Mon Sep 17 00:00:00 2001 From: haewon273 Date: Mon, 25 Aug 2025 12:19:06 +0900 Subject: [PATCH 01/16] =?UTF-8?q?=ED=98=B8=EB=B2=84=EB=A7=81=20=EC=BD=94?= =?UTF-8?q?=EB=93=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 70 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 70 insertions(+) diff --git a/README.md b/README.md index 8019425..68d238c 100644 --- a/README.md +++ b/README.md @@ -1 +1,71 @@ # aeroewha + +import math +import rclpy +from rclpy.node import Node +from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand + +class HoverControlNode(Node): + def __init__(self): + super().__init__('hover_control_node') + self.offboard_publisher = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', 10) + self.trajectory_publisher = self.create_publisher(TrajectorySetpoint, '/fmu/in/trajectory_setpoint', 10) + self.vehicle_command_publisher = self.create_publisher(VehicleCommand, '/fmu/in/vehicle_command', 10) + self.timer = self.create_timer(0.1, self.timer_callback) + self.get_logger().info("Hover Control Node Initialized") + self.armed = False + self.offboard_mode = False + + def timer_callback(self): + if not self.offboard_mode: + self.set_offboard_mode() + elif not self.armed: + self.arm_vehicle() + else: + self.publish_hover_setpoint() + + def set_offboard_mode(self): + msg = VehicleCommand() + msg.timestamp = self.get_clock().now().nanoseconds // 1000 + msg.command = VehicleCommand.VEHICLE_CMD_DO_SET_MODE + msg.param1 = 1.0 # Custom mode + msg.param2 = 6.0 # Offboard mode + msg.target_system = 1 + msg.target_component = 1 + self.vehicle_command_publisher.publish(msg) + self.offboard_mode = True + self.get_logger().info("Set to Offboard Mode") + + def arm_vehicle(self): + msg = VehicleCommand() + msg.timestamp = self.get_clock().now().nanoseconds // 1000 + msg.command = VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM + msg.param1 = 1.0 # Arm + msg.target_system = 1 + msg.target_component = 1 + self.vehicle_command_publisher.publish(msg) + self.armed = True + self.get_logger().info("Vehicle Armed") + + def publish_hover_setpoint(self): + offboard_msg = OffboardControlMode() + offboard_msg.timestamp = self.get_clock().now().nanoseconds // 1000 + offboard_msg.position = True + self.offboard_publisher.publish(offboard_msg) + + traj_msg = TrajectorySetpoint() + traj_msg.timestamp = self.get_clock().now().nanoseconds // 1000 + traj_msg.position = [0.0, 0.0, -2.0] # 2m 높이에서 호버링 + traj_msg.yaw = math.radians(0.0) # 0도 방향 + self.trajectory_publisher.publish(traj_msg) + self.get_logger().info("Publishing Hover Setpoint") + +def main(args=None): + rclpy.init(args=args) + node = HoverControlNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() From d7513e7ebca8a2f0b5e17b47b2139cec6063b711 Mon Sep 17 00:00:00 2001 From: haewon273 Date: Mon, 25 Aug 2025 13:53:59 +0900 Subject: [PATCH 02/16] Add files via upload --- hovering code.py | 69 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 69 insertions(+) create mode 100644 hovering code.py diff --git a/hovering code.py b/hovering code.py new file mode 100644 index 0000000..94c3637 --- /dev/null +++ b/hovering code.py @@ -0,0 +1,69 @@ +import math +import rclpy +from rclpy.node import Node +from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand + +class HoverControlNode(Node): + def __init__(self): + super().__init__('hover_control_node') + self.offboard_publisher = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', 10) + self.trajectory_publisher = self.create_publisher(TrajectorySetpoint, '/fmu/in/trajectory_setpoint', 10) + self.vehicle_command_publisher = self.create_publisher(VehicleCommand, '/fmu/in/vehicle_command', 10) + self.timer = self.create_timer(0.1, self.timer_callback) + self.get_logger().info("Hover Control Node Initialized") + self.armed = False + self.offboard_mode = False + + def timer_callback(self): + if not self.offboard_mode: + self.set_offboard_mode() + elif not self.armed: + self.arm_vehicle() + else: + self.publish_hover_setpoint() + + def set_offboard_mode(self): + msg = VehicleCommand() + msg.timestamp = self.get_clock().now().nanoseconds // 1000 + msg.command = VehicleCommand.VEHICLE_CMD_DO_SET_MODE + msg.param1 = 1.0 # Custom mode + msg.param2 = 6.0 # Offboard mode + msg.target_system = 1 + msg.target_component = 1 + self.vehicle_command_publisher.publish(msg) + self.offboard_mode = True + self.get_logger().info("Set to Offboard Mode") + + def arm_vehicle(self): + msg = VehicleCommand() + msg.timestamp = self.get_clock().now().nanoseconds // 1000 + msg.command = VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM + msg.param1 = 1.0 # Arm + msg.target_system = 1 + msg.target_component = 1 + self.vehicle_command_publisher.publish(msg) + self.armed = True + self.get_logger().info("Vehicle Armed") + + def publish_hover_setpoint(self): + offboard_msg = OffboardControlMode() + offboard_msg.timestamp = self.get_clock().now().nanoseconds // 1000 + offboard_msg.position = True + self.offboard_publisher.publish(offboard_msg) + + traj_msg = TrajectorySetpoint() + traj_msg.timestamp = self.get_clock().now().nanoseconds // 1000 + traj_msg.position = [0.0, 0.0, -2.0] # 2m 높이에서 호버링 + traj_msg.yaw = math.radians(0.0) # 0도 방향 + self.trajectory_publisher.publish(traj_msg) + self.get_logger().info("Publishing Hover Setpoint") + +def main(args=None): + rclpy.init(args=args) + node = HoverControlNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file From 74bf718313f3bac2041e84115a23a5690c71490d Mon Sep 17 00:00:00 2001 From: chaeyoung kim <152389483+chelsseeey@users.noreply.github.com> Date: Mon, 25 Aug 2025 13:54:29 +0900 Subject: [PATCH 03/16] Update README.md --- README.md | 68 ------------------------------------------------------- 1 file changed, 68 deletions(-) diff --git a/README.md b/README.md index 68d238c..8d45fa8 100644 --- a/README.md +++ b/README.md @@ -1,71 +1,3 @@ # aeroewha -import math -import rclpy -from rclpy.node import Node -from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand -class HoverControlNode(Node): - def __init__(self): - super().__init__('hover_control_node') - self.offboard_publisher = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', 10) - self.trajectory_publisher = self.create_publisher(TrajectorySetpoint, '/fmu/in/trajectory_setpoint', 10) - self.vehicle_command_publisher = self.create_publisher(VehicleCommand, '/fmu/in/vehicle_command', 10) - self.timer = self.create_timer(0.1, self.timer_callback) - self.get_logger().info("Hover Control Node Initialized") - self.armed = False - self.offboard_mode = False - - def timer_callback(self): - if not self.offboard_mode: - self.set_offboard_mode() - elif not self.armed: - self.arm_vehicle() - else: - self.publish_hover_setpoint() - - def set_offboard_mode(self): - msg = VehicleCommand() - msg.timestamp = self.get_clock().now().nanoseconds // 1000 - msg.command = VehicleCommand.VEHICLE_CMD_DO_SET_MODE - msg.param1 = 1.0 # Custom mode - msg.param2 = 6.0 # Offboard mode - msg.target_system = 1 - msg.target_component = 1 - self.vehicle_command_publisher.publish(msg) - self.offboard_mode = True - self.get_logger().info("Set to Offboard Mode") - - def arm_vehicle(self): - msg = VehicleCommand() - msg.timestamp = self.get_clock().now().nanoseconds // 1000 - msg.command = VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM - msg.param1 = 1.0 # Arm - msg.target_system = 1 - msg.target_component = 1 - self.vehicle_command_publisher.publish(msg) - self.armed = True - self.get_logger().info("Vehicle Armed") - - def publish_hover_setpoint(self): - offboard_msg = OffboardControlMode() - offboard_msg.timestamp = self.get_clock().now().nanoseconds // 1000 - offboard_msg.position = True - self.offboard_publisher.publish(offboard_msg) - - traj_msg = TrajectorySetpoint() - traj_msg.timestamp = self.get_clock().now().nanoseconds // 1000 - traj_msg.position = [0.0, 0.0, -2.0] # 2m 높이에서 호버링 - traj_msg.yaw = math.radians(0.0) # 0도 방향 - self.trajectory_publisher.publish(traj_msg) - self.get_logger().info("Publishing Hover Setpoint") - -def main(args=None): - rclpy.init(args=args) - node = HoverControlNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() From 19da544b9533ef490398b93fdb240a11e82e3a23 Mon Sep 17 00:00:00 2001 From: haewon273 Date: Mon, 25 Aug 2025 14:22:22 +0900 Subject: [PATCH 04/16] Add files via upload From 7b4b4c429edcd240240fc07dfbff8cad49b5cd16 Mon Sep 17 00:00:00 2001 From: haewon273 Date: Mon, 25 Aug 2025 14:25:16 +0900 Subject: [PATCH 05/16] Add files via upload --- hovering code N. py.txt | 75 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 75 insertions(+) create mode 100644 hovering code N. py.txt diff --git a/hovering code N. py.txt b/hovering code N. py.txt new file mode 100644 index 0000000..ad058c0 --- /dev/null +++ b/hovering code N. py.txt @@ -0,0 +1,75 @@ +import math +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped +from mavros_msgs.srv import CommandBool, SetMode +from mavros_msgs.msg import State + +class HoverMavrosNode(Node): + def __init__(self): + super().__init__('hover_mavros_node') + + # MAVROS 상태 구독 + self.state_sub = self.create_subscription(State, '/mavros2/state', self.state_callback, 10) + self.current_state = State() + + # 호버링 위치 퍼블리시 + self.pose_pub = self.create_publisher(PoseStamped, '/mavros2/setpoint_position/local', 10) + + # 서비스 클라이언트 + self.arm_client = self.create_client(CommandBool, '/mavros2/cmd/arming') + self.mode_client = self.create_client(SetMode, '/mavros2/set_mode') + + self.timer = self.create_timer(0.1, self.timer_callback) + self.armed = False + self.offboard_mode = False + self.pose_setpoint = PoseStamped() + self.pose_setpoint.pose.position.x = 0.0 + self.pose_setpoint.pose.position.y = 0.0 + self.pose_setpoint.pose.position.z = 2.0 # 2m 호버링 + self.get_logger().info("Hover MAVROS Node Initialized") + + def state_callback(self, msg): + self.current_state = msg + + def timer_callback(self): + # Offboard 모드 설정 + if not self.offboard_mode: + self.set_offboard_mode() + # Arm + elif not self.armed: + self.arm_vehicle() + else: + # 호버링 setpoint 전송 + self.pose_pub.publish(self.pose_setpoint) + self.get_logger().info("Publishing Hover Setpoint") + + def set_offboard_mode(self): + if self.mode_client.service_is_ready(): + req = SetMode.Request() + req.custom_mode = "OFFBOARD" + future = self.mode_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result().mode_sent: + self.offboard_mode = True + self.get_logger().info("Set to OFFBOARD mode") + + def arm_vehicle(self): + if self.arm_client.service_is_ready(): + req = CommandBool.Request() + req.value = True + future = self.arm_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result().success: + self.armed = True + self.get_logger().info("Vehicle Armed") + +def main(args=None): + rclpy.init(args=args) + node = HoverMavrosNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == "__main__": + main() \ No newline at end of file From 6cc0a6fdf6dfd01569c82c5d590048cace0a45ee Mon Sep 17 00:00:00 2001 From: haewon273 Date: Mon, 25 Aug 2025 14:49:15 +0900 Subject: [PATCH 06/16] Add files via upload --- control allocation code.py | 57 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 57 insertions(+) create mode 100644 control allocation code.py diff --git a/control allocation code.py b/control allocation code.py new file mode 100644 index 0000000..1a17726 --- /dev/null +++ b/control allocation code.py @@ -0,0 +1,57 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import TwistStamped +from std_msgs.msg import Header + +class DroneController(Node): + + def __init__(self): + super().__init__('drone_controller') + + # /mavros/setpoint_raw/attitude 토픽을 발행하는 퍼블리셔 생성 + self.publisher_ = self.create_publisher(TwistStamped, '/mavros/setpoint_raw/attitude', 10) + + # 0.02초(50Hz)마다 publish_attitude_thrust 함수 호출 + timer_period = 0.02 # seconds + self.timer = self.create_timer(timer_period, self.publish_attitude_thrust) + + # 목표 추력 (0.0 ~ 1.0) + self.thrust = 0.6 # 이륙에 필요한 기본 추력 + + self.get_logger().info('드론 제어 노드가 시작되었습니다.') + + def publish_attitude_thrust(self): + """MAVROS에 자세 및 추력 메시지를 발행합니다.""" + + msg = TwistStamped() + msg.header = Header() + msg.header.stamp = self.get_clock().now().to_msg() + + # ---------- 자세(Attitude) 및 추력(Thrust) 설정 ---------- + + # 드론의 추력 제어 (위로 올라가는 힘) + # linear.z 값을 0.0~1.0 사이로 설정하여 드론의 고도를 조절 + msg.twist.linear.z = self.thrust + + # 드론의 회전(Roll, Pitch, Yaw) 제어 + # 0.0으로 설정하면 안정적인 자세를 유지 + msg.twist.angular.x = 0.0 # Roll (좌우 기울기) + msg.twist.angular.y = 0.0 # Pitch (앞뒤 기울기) + msg.twist.angular.z = 0.0 # Yaw (수평 회전) + + # 메시지 발행 + self.publisher_.publish(msg) + + # 로그 출력 + self.get_logger().info('자세/추력 메시지를 발행했습니다.') + + +def main(args=None): + rclpy.init(args=args) + drone_controller = DroneController() + rclpy.spin(drone_controller) + drone_controller.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file From 6b17ebb70bad67cf8094b9e5858d8dcdd6037a49 Mon Sep 17 00:00:00 2001 From: haewon273 Date: Sat, 22 Nov 2025 23:04:24 +0900 Subject: [PATCH 07/16] Add files via upload --- takeoff_n.py | 137 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 137 insertions(+) create mode 100644 takeoff_n.py diff --git a/takeoff_n.py b/takeoff_n.py new file mode 100644 index 0000000..ad088ab --- /dev/null +++ b/takeoff_n.py @@ -0,0 +1,137 @@ +// file: motor_gain_pre_mixer.cpp + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + + +class MotorGainPreMixer : public ModuleBase, public ModuleParams +{ +public: + MotorGainPreMixer() : + ModuleParams(nullptr), + _controls_sub(ORB_ID(actuator_controls_0)), + _controls_pub(ORB_ID(actuator_controls_0)) + {} + + ~MotorGainPreMixer() override {} + + static int task_spawn(int argc, char *argv[]) + { + MotorGainPreMixer *instance = new MotorGainPreMixer(); + + if (!instance) return PX4_ERROR; + + _object.store(instance); + _task_id = px4_task_spawn_cmd( + "motor_gain_pre_mixer", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + (px4_main_t)&run_trampoline, + nullptr + ); + + if (_task_id < 0) { + delete instance; + return PX4_ERROR; + } + + return PX4_OK; + } + + static MotorGainPreMixer *instantiate(int argc, char *argv[]) { return new MotorGainPreMixer(); } + + void run() override + { + PX4_INFO("MotorGainPreMixer started."); + + actuator_controls_s controls_in{}; + actuator_controls_s controls_out{}; + + while (!should_exit()) { + + if (_controls_sub.update(&controls_in)) { + + // Copy timestamp & default values + controls_out = controls_in; + + // Motor mapping: + // control[0] = roll + // control[1] = pitch + // control[2] = yaw + // control[3] = collective thrust (0~1 normalized) + + // Mixer combines these into motor outputs, + // but we want to scale per motor BEFORE mixer. + // + // In PX4, individual motors are not separated here. + // So we modify the thrust component ONLY. + + float thrust = controls_in.control[3]; + + // Apply gain per motor by approximating desired thrust correction: + // mixer will generate 4 motor commands from roll/pitch/yaw/thrust + // We modify thrust based on average gain or adjust control set. + + float g1 = _g1.get(); + float g2 = _g2.get(); + float g3 = _g3.get(); + float g4 = _g4.get(); + + // 하나의 thrust 값으로 4개 모터의 상대 추력을 보정하려면 + // 평균 gain 적용이 가장 논리적임. + float g_avg = (g1 + g2 + g3 + g4) * 0.25f; + + controls_out.control[3] = thrust * g_avg; + + controls_out.timestamp = hrt_absolute_time(); + + _controls_pub.publish(controls_out); + } + + px4_usleep(2000); // ~500 Hz loop + } + + PX4_INFO("MotorGainPreMixer stopped."); + } + + static int custom_command(int argc, char *argv[]) { return print_usage("Unknown command"); } + static int print_usage(const char *reason) + { + if (reason) PX4_WARN("%s", reason); + + PRINT_MODULE_USAGE_NAME("motor_gain_pre_mixer", "custom"); + PRINT_MODULE_USAGE_COMMAND("start"); + return 0; + } + + +private: + + DEFINE_PARAMETERS( + (ParamFloat) _g1, + (ParamFloat) _g2, + (ParamFloat) _g3, + (ParamFloat) _g4 + ); + + uORB::Subscription _controls_sub; + uORB::Publication _controls_pub; +}; + + +MotorGainPreMixer *MotorGainPreMixer::_object = nullptr; + +extern "C" __EXPORT int motor_gain_pre_mixer_main(int argc, char *argv[]) +{ + return MotorGainPreMixer::main(argc, argv); +} From 6aab030ada342bae8b203a34e0674d6d09aefb94 Mon Sep 17 00:00:00 2001 From: haewon273 Date: Wed, 26 Nov 2025 15:53:20 +0900 Subject: [PATCH 08/16] Add files via upload --- Individual_moter_control_code.py | 177 +++++++++++++++++++++++++++++++ 1 file changed, 177 insertions(+) create mode 100644 Individual_moter_control_code.py 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); +} From 9ae62bb2de49c0faf6adb995df8b696afc676d18 Mon Sep 17 00:00:00 2001 From: haewon273 Date: Wed, 26 Nov 2025 15:53:47 +0900 Subject: [PATCH 09/16] Delete control allocation code.py --- control allocation code.py | 57 -------------------------------------- 1 file changed, 57 deletions(-) delete mode 100644 control allocation code.py diff --git a/control allocation code.py b/control allocation code.py deleted file mode 100644 index 1a17726..0000000 --- a/control allocation code.py +++ /dev/null @@ -1,57 +0,0 @@ -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import TwistStamped -from std_msgs.msg import Header - -class DroneController(Node): - - def __init__(self): - super().__init__('drone_controller') - - # /mavros/setpoint_raw/attitude 토픽을 발행하는 퍼블리셔 생성 - self.publisher_ = self.create_publisher(TwistStamped, '/mavros/setpoint_raw/attitude', 10) - - # 0.02초(50Hz)마다 publish_attitude_thrust 함수 호출 - timer_period = 0.02 # seconds - self.timer = self.create_timer(timer_period, self.publish_attitude_thrust) - - # 목표 추력 (0.0 ~ 1.0) - self.thrust = 0.6 # 이륙에 필요한 기본 추력 - - self.get_logger().info('드론 제어 노드가 시작되었습니다.') - - def publish_attitude_thrust(self): - """MAVROS에 자세 및 추력 메시지를 발행합니다.""" - - msg = TwistStamped() - msg.header = Header() - msg.header.stamp = self.get_clock().now().to_msg() - - # ---------- 자세(Attitude) 및 추력(Thrust) 설정 ---------- - - # 드론의 추력 제어 (위로 올라가는 힘) - # linear.z 값을 0.0~1.0 사이로 설정하여 드론의 고도를 조절 - msg.twist.linear.z = self.thrust - - # 드론의 회전(Roll, Pitch, Yaw) 제어 - # 0.0으로 설정하면 안정적인 자세를 유지 - msg.twist.angular.x = 0.0 # Roll (좌우 기울기) - msg.twist.angular.y = 0.0 # Pitch (앞뒤 기울기) - msg.twist.angular.z = 0.0 # Yaw (수평 회전) - - # 메시지 발행 - self.publisher_.publish(msg) - - # 로그 출력 - self.get_logger().info('자세/추력 메시지를 발행했습니다.') - - -def main(args=None): - rclpy.init(args=args) - drone_controller = DroneController() - rclpy.spin(drone_controller) - drone_controller.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file From 47e32164a74b1d8d819d37e53763f1088227058b Mon Sep 17 00:00:00 2001 From: haewon273 Date: Wed, 26 Nov 2025 15:54:00 +0900 Subject: [PATCH 10/16] Delete hovering code N. py.txt --- hovering code N. py.txt | 75 ----------------------------------------- 1 file changed, 75 deletions(-) delete mode 100644 hovering code N. py.txt diff --git a/hovering code N. py.txt b/hovering code N. py.txt deleted file mode 100644 index ad058c0..0000000 --- a/hovering code N. py.txt +++ /dev/null @@ -1,75 +0,0 @@ -import math -import rclpy -from rclpy.node import Node -from geometry_msgs.msg import PoseStamped -from mavros_msgs.srv import CommandBool, SetMode -from mavros_msgs.msg import State - -class HoverMavrosNode(Node): - def __init__(self): - super().__init__('hover_mavros_node') - - # MAVROS 상태 구독 - self.state_sub = self.create_subscription(State, '/mavros2/state', self.state_callback, 10) - self.current_state = State() - - # 호버링 위치 퍼블리시 - self.pose_pub = self.create_publisher(PoseStamped, '/mavros2/setpoint_position/local', 10) - - # 서비스 클라이언트 - self.arm_client = self.create_client(CommandBool, '/mavros2/cmd/arming') - self.mode_client = self.create_client(SetMode, '/mavros2/set_mode') - - self.timer = self.create_timer(0.1, self.timer_callback) - self.armed = False - self.offboard_mode = False - self.pose_setpoint = PoseStamped() - self.pose_setpoint.pose.position.x = 0.0 - self.pose_setpoint.pose.position.y = 0.0 - self.pose_setpoint.pose.position.z = 2.0 # 2m 호버링 - self.get_logger().info("Hover MAVROS Node Initialized") - - def state_callback(self, msg): - self.current_state = msg - - def timer_callback(self): - # Offboard 모드 설정 - if not self.offboard_mode: - self.set_offboard_mode() - # Arm - elif not self.armed: - self.arm_vehicle() - else: - # 호버링 setpoint 전송 - self.pose_pub.publish(self.pose_setpoint) - self.get_logger().info("Publishing Hover Setpoint") - - def set_offboard_mode(self): - if self.mode_client.service_is_ready(): - req = SetMode.Request() - req.custom_mode = "OFFBOARD" - future = self.mode_client.call_async(req) - rclpy.spin_until_future_complete(self, future) - if future.result().mode_sent: - self.offboard_mode = True - self.get_logger().info("Set to OFFBOARD mode") - - def arm_vehicle(self): - if self.arm_client.service_is_ready(): - req = CommandBool.Request() - req.value = True - future = self.arm_client.call_async(req) - rclpy.spin_until_future_complete(self, future) - if future.result().success: - self.armed = True - self.get_logger().info("Vehicle Armed") - -def main(args=None): - rclpy.init(args=args) - node = HoverMavrosNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == "__main__": - main() \ No newline at end of file From 093ceb52cb46fd024d0e66a5345872de31626729 Mon Sep 17 00:00:00 2001 From: haewon273 Date: Wed, 26 Nov 2025 15:54:10 +0900 Subject: [PATCH 11/16] Delete takeoff_n.py --- takeoff_n.py | 137 --------------------------------------------------- 1 file changed, 137 deletions(-) delete mode 100644 takeoff_n.py diff --git a/takeoff_n.py b/takeoff_n.py deleted file mode 100644 index ad088ab..0000000 --- a/takeoff_n.py +++ /dev/null @@ -1,137 +0,0 @@ -// file: motor_gain_pre_mixer.cpp - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - - -class MotorGainPreMixer : public ModuleBase, public ModuleParams -{ -public: - MotorGainPreMixer() : - ModuleParams(nullptr), - _controls_sub(ORB_ID(actuator_controls_0)), - _controls_pub(ORB_ID(actuator_controls_0)) - {} - - ~MotorGainPreMixer() override {} - - static int task_spawn(int argc, char *argv[]) - { - MotorGainPreMixer *instance = new MotorGainPreMixer(); - - if (!instance) return PX4_ERROR; - - _object.store(instance); - _task_id = px4_task_spawn_cmd( - "motor_gain_pre_mixer", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2000, - (px4_main_t)&run_trampoline, - nullptr - ); - - if (_task_id < 0) { - delete instance; - return PX4_ERROR; - } - - return PX4_OK; - } - - static MotorGainPreMixer *instantiate(int argc, char *argv[]) { return new MotorGainPreMixer(); } - - void run() override - { - PX4_INFO("MotorGainPreMixer started."); - - actuator_controls_s controls_in{}; - actuator_controls_s controls_out{}; - - while (!should_exit()) { - - if (_controls_sub.update(&controls_in)) { - - // Copy timestamp & default values - controls_out = controls_in; - - // Motor mapping: - // control[0] = roll - // control[1] = pitch - // control[2] = yaw - // control[3] = collective thrust (0~1 normalized) - - // Mixer combines these into motor outputs, - // but we want to scale per motor BEFORE mixer. - // - // In PX4, individual motors are not separated here. - // So we modify the thrust component ONLY. - - float thrust = controls_in.control[3]; - - // Apply gain per motor by approximating desired thrust correction: - // mixer will generate 4 motor commands from roll/pitch/yaw/thrust - // We modify thrust based on average gain or adjust control set. - - float g1 = _g1.get(); - float g2 = _g2.get(); - float g3 = _g3.get(); - float g4 = _g4.get(); - - // 하나의 thrust 값으로 4개 모터의 상대 추력을 보정하려면 - // 평균 gain 적용이 가장 논리적임. - float g_avg = (g1 + g2 + g3 + g4) * 0.25f; - - controls_out.control[3] = thrust * g_avg; - - controls_out.timestamp = hrt_absolute_time(); - - _controls_pub.publish(controls_out); - } - - px4_usleep(2000); // ~500 Hz loop - } - - PX4_INFO("MotorGainPreMixer stopped."); - } - - static int custom_command(int argc, char *argv[]) { return print_usage("Unknown command"); } - static int print_usage(const char *reason) - { - if (reason) PX4_WARN("%s", reason); - - PRINT_MODULE_USAGE_NAME("motor_gain_pre_mixer", "custom"); - PRINT_MODULE_USAGE_COMMAND("start"); - return 0; - } - - -private: - - DEFINE_PARAMETERS( - (ParamFloat) _g1, - (ParamFloat) _g2, - (ParamFloat) _g3, - (ParamFloat) _g4 - ); - - uORB::Subscription _controls_sub; - uORB::Publication _controls_pub; -}; - - -MotorGainPreMixer *MotorGainPreMixer::_object = nullptr; - -extern "C" __EXPORT int motor_gain_pre_mixer_main(int argc, char *argv[]) -{ - return MotorGainPreMixer::main(argc, argv); -} From d2f463084893743034148cc0dc334059a7d1f603 Mon Sep 17 00:00:00 2001 From: haewon273 Date: Wed, 26 Nov 2025 15:54:22 +0900 Subject: [PATCH 12/16] Delete README.md --- README.md | 3 --- 1 file changed, 3 deletions(-) delete mode 100644 README.md diff --git a/README.md b/README.md deleted file mode 100644 index 8d45fa8..0000000 --- a/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# aeroewha - - From e9df37c83ce8722b38cdee6b4feef280bd69a0f7 Mon Sep 17 00:00:00 2001 From: haewon273 Date: Wed, 26 Nov 2025 15:54:33 +0900 Subject: [PATCH 13/16] Delete hovering code.py --- hovering code.py | 69 ------------------------------------------------ 1 file changed, 69 deletions(-) delete mode 100644 hovering code.py diff --git a/hovering code.py b/hovering code.py deleted file mode 100644 index 94c3637..0000000 --- a/hovering code.py +++ /dev/null @@ -1,69 +0,0 @@ -import math -import rclpy -from rclpy.node import Node -from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand - -class HoverControlNode(Node): - def __init__(self): - super().__init__('hover_control_node') - self.offboard_publisher = self.create_publisher(OffboardControlMode, '/fmu/in/offboard_control_mode', 10) - self.trajectory_publisher = self.create_publisher(TrajectorySetpoint, '/fmu/in/trajectory_setpoint', 10) - self.vehicle_command_publisher = self.create_publisher(VehicleCommand, '/fmu/in/vehicle_command', 10) - self.timer = self.create_timer(0.1, self.timer_callback) - self.get_logger().info("Hover Control Node Initialized") - self.armed = False - self.offboard_mode = False - - def timer_callback(self): - if not self.offboard_mode: - self.set_offboard_mode() - elif not self.armed: - self.arm_vehicle() - else: - self.publish_hover_setpoint() - - def set_offboard_mode(self): - msg = VehicleCommand() - msg.timestamp = self.get_clock().now().nanoseconds // 1000 - msg.command = VehicleCommand.VEHICLE_CMD_DO_SET_MODE - msg.param1 = 1.0 # Custom mode - msg.param2 = 6.0 # Offboard mode - msg.target_system = 1 - msg.target_component = 1 - self.vehicle_command_publisher.publish(msg) - self.offboard_mode = True - self.get_logger().info("Set to Offboard Mode") - - def arm_vehicle(self): - msg = VehicleCommand() - msg.timestamp = self.get_clock().now().nanoseconds // 1000 - msg.command = VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM - msg.param1 = 1.0 # Arm - msg.target_system = 1 - msg.target_component = 1 - self.vehicle_command_publisher.publish(msg) - self.armed = True - self.get_logger().info("Vehicle Armed") - - def publish_hover_setpoint(self): - offboard_msg = OffboardControlMode() - offboard_msg.timestamp = self.get_clock().now().nanoseconds // 1000 - offboard_msg.position = True - self.offboard_publisher.publish(offboard_msg) - - traj_msg = TrajectorySetpoint() - traj_msg.timestamp = self.get_clock().now().nanoseconds // 1000 - traj_msg.position = [0.0, 0.0, -2.0] # 2m 높이에서 호버링 - traj_msg.yaw = math.radians(0.0) # 0도 방향 - self.trajectory_publisher.publish(traj_msg) - self.get_logger().info("Publishing Hover Setpoint") - -def main(args=None): - rclpy.init(args=args) - node = HoverControlNode() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file From 53a849bf25000c223ea01eb542adcbed97b60714 Mon Sep 17 00:00:00 2001 From: haewon273 Date: Thu, 27 Nov 2025 16:02:58 +0900 Subject: [PATCH 14/16] Add files via upload --- failsafe_code.py | 180 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 180 insertions(+) create mode 100644 failsafe_code.py diff --git a/failsafe_code.py b/failsafe_code.py new file mode 100644 index 0000000..2f166fe --- /dev/null +++ b/failsafe_code.py @@ -0,0 +1,180 @@ +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class FailSafeHover : public ModuleBase +{ +public: + FailSafeHover() + : _sub_lpos(ORB_ID(vehicle_local_position)), + _sub_gpos(ORB_ID(vehicle_global_position)), + _sub_gps(ORB_ID(vehicle_gps_position)), + _sub_status(ORB_ID(vehicle_status)), + _sub_batt(ORB_ID(battery_status)), + _pub_sp(ORB_ID(vehicle_rates_setpoint)) + {} + + void run() override; + + static int task_spawn(int argc, char *argv[]) + { + FailSafeHover *instance = new FailSafeHover(); + if (!instance) return PX4_ERROR; + + _object.store(instance); + + px4_task_spawn_cmd("failsafe_hover", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2200, + (px4_main_t)&FailSafeHover::task_main_trampoline, + nullptr); + return PX4_OK; + } + + static FailSafeHover *instantiate(int, char *[]) { return new FailSafeHover(); } + static int custom_command(int, char *[]) { return print_usage("unknown cmd"); } + + static int print_usage(const char *) { + PRINT_MODULE_USAGE_NAME("failsafe_hover", "module"); + PRINT_MODULE_USAGE_COMMAND("start"); + return 0; + } + +private: + + // Subscriptions + uORB::Subscription _sub_lpos; + uORB::Subscription _sub_gpos; + uORB::Subscription _sub_gps; + uORB::Subscription _sub_status; + uORB::Subscription _sub_batt; + + // Publication + uORB::Publication _pub_sp; + + bool _failsafe_active{false}; + float _target_alt{0.0f}; // absolute z position + float _failsafe_hover_z{-10.f}; // default hover altitude (–10m = 약 10m 위) +}; + +// ------------------------------------------------------------- + +void FailSafeHover::run() +{ + PX4_INFO("Failsafe Hover module started"); + + vehicle_local_position_s lpos{}; + vehicle_status_s status{}; + battery_status_s batt{}; + vehicle_gps_position_s gps{}; + vehicle_global_position_s gpos{}; + + while (!should_exit()) { + + _sub_status.update(&status); + _sub_batt.update(&batt); + _sub_gps.update(&gps); + _sub_gpos.update(&gpos); + + bool trigger = false; + + // 1) 통신 두절(RC link lost) + if (status.rc_signal_lost) { + PX4_WARN("Failsafe: RC signal lost!"); + trigger = true; + } + + // 2) GPS 오류 + if (gps.fix_type < 3 || gps.eph > 2.5f) { + PX4_WARN("Failsafe: GPS error!"); + trigger = true; + } + + // 3) 배터리 부족 + if (batt.voltage_filtered_v < 10.5f) { + PX4_WARN("Failsafe: Low battery!"); + trigger = true; + } + + // 4) 금지 구역 진입 (Radius 100m 예시) + const float nofly_lat = 37.123456; // 금지구역 중심 + const float nofly_lon = 127.123456; + const float radius_m = 100.0f; + + float dlat = (float)(gps.lat * 1e-7 - nofly_lat) * 111320.0f; + float dlon = (float)(gps.lon * 1e-7 - nofly_lon) * 111320.0f * cosf(nofly_lat * M_PI / 180.0f); + + float dist = sqrtf(dlat * dlat + dlon * dlon); + + if (dist < radius_m) { + PX4_WARN("Failsafe: Entered no-fly zone!"); + trigger = true; + } + + // ------------------------- + // Failsafe Mode Enable + // ------------------------- + if (trigger && !_failsafe_active) { + + PX4_WARN("Failsafe HOVER engaged!"); + + if (_sub_lpos.copy(&lpos) == PX4_OK) { + // 현재 z 기준으로 특정 절대고도 설정 + _target_alt = _failsafe_hover_z; + } + + _failsafe_active = true; + } + + // ------------------------- + // Failsafe Hover Control + // ------------------------- + if (_failsafe_active) { + + if (_sub_lpos.copy(&lpos) == PX4_OK) { + + float error = (_target_alt - lpos.z); + + // 단순 P 제어 + 호버 + float thrust = error * 0.45f + 0.55f; + + if (thrust < 0.3f) thrust = 0.3f; + if (thrust > 0.9f) thrust = 0.9f; + + vehicle_rates_setpoint_s sp{}; + sp.timestamp = hrt_absolute_time(); + + sp.roll = 0.f; + sp.pitch = 0.f; + sp.yaw = 0.f; + sp.thrust_body[2] = -thrust; // 위쪽 힘은 음수 + + _pub_sp.publish(sp); + } + } + + px4_usleep(10_ms); + } + + PX4_INFO("Failsafe Hover module exiting"); +} + +// ------------------------------------------------------------- + +extern "C" __EXPORT int failsafe_hover_main(int argc, char *argv[]) +{ + return FailSafeHover::main(argc, argv); +} From 2f39a9fa8b3dab0dd4a344fa8d6c8ee5b50bc07d Mon Sep 17 00:00:00 2001 From: haewon273 Date: Thu, 27 Nov 2025 20:46:20 +0900 Subject: [PATCH 15/16] Delete failsafe_code.py --- failsafe_code.py | 180 ----------------------------------------------- 1 file changed, 180 deletions(-) delete mode 100644 failsafe_code.py diff --git a/failsafe_code.py b/failsafe_code.py deleted file mode 100644 index 2f166fe..0000000 --- a/failsafe_code.py +++ /dev/null @@ -1,180 +0,0 @@ -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -class FailSafeHover : public ModuleBase -{ -public: - FailSafeHover() - : _sub_lpos(ORB_ID(vehicle_local_position)), - _sub_gpos(ORB_ID(vehicle_global_position)), - _sub_gps(ORB_ID(vehicle_gps_position)), - _sub_status(ORB_ID(vehicle_status)), - _sub_batt(ORB_ID(battery_status)), - _pub_sp(ORB_ID(vehicle_rates_setpoint)) - {} - - void run() override; - - static int task_spawn(int argc, char *argv[]) - { - FailSafeHover *instance = new FailSafeHover(); - if (!instance) return PX4_ERROR; - - _object.store(instance); - - px4_task_spawn_cmd("failsafe_hover", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2200, - (px4_main_t)&FailSafeHover::task_main_trampoline, - nullptr); - return PX4_OK; - } - - static FailSafeHover *instantiate(int, char *[]) { return new FailSafeHover(); } - static int custom_command(int, char *[]) { return print_usage("unknown cmd"); } - - static int print_usage(const char *) { - PRINT_MODULE_USAGE_NAME("failsafe_hover", "module"); - PRINT_MODULE_USAGE_COMMAND("start"); - return 0; - } - -private: - - // Subscriptions - uORB::Subscription _sub_lpos; - uORB::Subscription _sub_gpos; - uORB::Subscription _sub_gps; - uORB::Subscription _sub_status; - uORB::Subscription _sub_batt; - - // Publication - uORB::Publication _pub_sp; - - bool _failsafe_active{false}; - float _target_alt{0.0f}; // absolute z position - float _failsafe_hover_z{-10.f}; // default hover altitude (–10m = 약 10m 위) -}; - -// ------------------------------------------------------------- - -void FailSafeHover::run() -{ - PX4_INFO("Failsafe Hover module started"); - - vehicle_local_position_s lpos{}; - vehicle_status_s status{}; - battery_status_s batt{}; - vehicle_gps_position_s gps{}; - vehicle_global_position_s gpos{}; - - while (!should_exit()) { - - _sub_status.update(&status); - _sub_batt.update(&batt); - _sub_gps.update(&gps); - _sub_gpos.update(&gpos); - - bool trigger = false; - - // 1) 통신 두절(RC link lost) - if (status.rc_signal_lost) { - PX4_WARN("Failsafe: RC signal lost!"); - trigger = true; - } - - // 2) GPS 오류 - if (gps.fix_type < 3 || gps.eph > 2.5f) { - PX4_WARN("Failsafe: GPS error!"); - trigger = true; - } - - // 3) 배터리 부족 - if (batt.voltage_filtered_v < 10.5f) { - PX4_WARN("Failsafe: Low battery!"); - trigger = true; - } - - // 4) 금지 구역 진입 (Radius 100m 예시) - const float nofly_lat = 37.123456; // 금지구역 중심 - const float nofly_lon = 127.123456; - const float radius_m = 100.0f; - - float dlat = (float)(gps.lat * 1e-7 - nofly_lat) * 111320.0f; - float dlon = (float)(gps.lon * 1e-7 - nofly_lon) * 111320.0f * cosf(nofly_lat * M_PI / 180.0f); - - float dist = sqrtf(dlat * dlat + dlon * dlon); - - if (dist < radius_m) { - PX4_WARN("Failsafe: Entered no-fly zone!"); - trigger = true; - } - - // ------------------------- - // Failsafe Mode Enable - // ------------------------- - if (trigger && !_failsafe_active) { - - PX4_WARN("Failsafe HOVER engaged!"); - - if (_sub_lpos.copy(&lpos) == PX4_OK) { - // 현재 z 기준으로 특정 절대고도 설정 - _target_alt = _failsafe_hover_z; - } - - _failsafe_active = true; - } - - // ------------------------- - // Failsafe Hover Control - // ------------------------- - if (_failsafe_active) { - - if (_sub_lpos.copy(&lpos) == PX4_OK) { - - float error = (_target_alt - lpos.z); - - // 단순 P 제어 + 호버 - float thrust = error * 0.45f + 0.55f; - - if (thrust < 0.3f) thrust = 0.3f; - if (thrust > 0.9f) thrust = 0.9f; - - vehicle_rates_setpoint_s sp{}; - sp.timestamp = hrt_absolute_time(); - - sp.roll = 0.f; - sp.pitch = 0.f; - sp.yaw = 0.f; - sp.thrust_body[2] = -thrust; // 위쪽 힘은 음수 - - _pub_sp.publish(sp); - } - } - - px4_usleep(10_ms); - } - - PX4_INFO("Failsafe Hover module exiting"); -} - -// ------------------------------------------------------------- - -extern "C" __EXPORT int failsafe_hover_main(int argc, char *argv[]) -{ - return FailSafeHover::main(argc, argv); -} From 464ae0ea778467698350ad7e1c3bee2d030fcab6 Mon Sep 17 00:00:00 2001 From: haewon273 Date: Thu, 27 Nov 2025 20:46:58 +0900 Subject: [PATCH 16/16] Add files via upload --- failsafe_code.py | 184 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 184 insertions(+) create mode 100644 failsafe_code.py 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); + } +} +