From 861e06dfd7e317a426cb0868e5824b9d92987dc8 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 25 Sep 2020 11:36:58 -0400 Subject: [PATCH] mavlink: handle receiving GENERATOR_STATUS - only published (ORB_ID(generator_status)) and logged for now --- msg/CMakeLists.txt | 1 + msg/generator_status.msg | 44 ++++++++++++++++++++++++ msg/tools/uorb_rtps_message_ids.yaml | 2 ++ src/modules/logger/logged_topics.cpp | 3 +- src/modules/mavlink/mavlink_receiver.cpp | 26 ++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 9 +++-- 6 files changed, 81 insertions(+), 4 deletions(-) create mode 100644 msg/generator_status.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index a0ee67cb41..ce9b8ea7af 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -65,6 +65,7 @@ set(msg_files estimator_states.msg estimator_status.msg follow_target.msg + generator_status.msg geofence_result.msg gps_dump.msg gps_inject_data.msg diff --git a/msg/generator_status.msg b/msg/generator_status.msg new file mode 100644 index 0000000000..7ba9b40223 --- /dev/null +++ b/msg/generator_status.msg @@ -0,0 +1,44 @@ +uint64 timestamp # time since system start (microseconds) + + +uint64 STATUS_FLAG_OFF = 1 # Generator is off. +uint64 STATUS_FLAG_READY = 2 # Generator is ready to start generating power. +uint64 STATUS_FLAG_GENERATING = 4 # Generator is generating power. +uint64 STATUS_FLAG_CHARGING = 8 # Generator is charging the batteries (generating enough power to charge and provide the load). +uint64 STATUS_FLAG_REDUCED_POWER = 16 # Generator is operating at a reduced maximum power. +uint64 STATUS_FLAG_MAXPOWER = 32 # Generator is providing the maximum output. +uint64 STATUS_FLAG_OVERTEMP_WARNING = 64 # Generator is near the maximum operating temperature, cooling is insufficient. +uint64 STATUS_FLAG_OVERTEMP_FAULT = 128 # Generator hit the maximum operating temperature and shutdown. +uint64 STATUS_FLAG_ELECTRONICS_OVERTEMP_WARNING = 256 # Power electronics are near the maximum operating temperature, cooling is insufficient. +uint64 STATUS_FLAG_ELECTRONICS_OVERTEMP_FAULT = 512 # Power electronics hit the maximum operating temperature and shutdown. +uint64 STATUS_FLAG_ELECTRONICS_FAULT = 1024 # Power electronics experienced a fault and shutdown. +uint64 STATUS_FLAG_POWERSOURCE_FAULT = 2048 # The power source supplying the generator failed e.g. mechanical generator stopped, tether is no longer providing power, solar cell is in shade, hydrogen reaction no longer happening. +uint64 STATUS_FLAG_COMMUNICATION_WARNING = 4096 # Generator controller having communication problems. +uint64 STATUS_FLAG_COOLING_WARNING = 8192 # Power electronic or generator cooling system error. +uint64 STATUS_FLAG_POWER_RAIL_FAULT = 16384 # Generator controller power rail experienced a fault. +uint64 STATUS_FLAG_OVERCURRENT_FAULT = 32768 # Generator controller exceeded the overcurrent threshold and shutdown to prevent damage. +uint64 STATUS_FLAG_BATTERY_OVERCHARGE_CURRENT_FAULT = 65536 # Generator controller detected a high current going into the batteries and shutdown to prevent battery damage. | +uint64 STATUS_FLAG_OVERVOLTAGE_FAULT = 131072 # Generator controller exceeded it's overvoltage threshold and shutdown to prevent it exceeding the voltage rating. +uint64 STATUS_FLAG_BATTERY_UNDERVOLT_FAULT = 262144 # Batteries are under voltage (generator will not start). +uint64 STATUS_FLAG_START_INHIBITED = 524288 # Generator start is inhibited by e.g. a safety switch. +uint64 STATUS_FLAG_MAINTENANCE_REQUIRED = 1048576 # Generator requires maintenance. +uint64 STATUS_FLAG_WARMING_UP = 2097152 # Generator is not ready to generate yet. +uint64 STATUS_FLAG_IDLE = 4194304 # Generator is idle. + +uint64 status # Status flags + + +float32 battery_current # [A] Current into/out of battery. Positive for out. Negative for in. NaN: field not provided. +float32 load_current # [A] Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided +float32 power_generated # [W] The power being generated. NaN: field not provided +float32 bus_voltage # [V] Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus. +float32 bat_current_setpoint # [A] The target battery current. Positive for out. Negative for in. NaN: field not provided + +uint32 runtime # [s] Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided. + +int32 time_until_maintenance # [s] Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided. + +uint16 generator_speed # [rpm] Speed of electrical generator or alternator. UINT16_MAX: field not provided. + +int16 rectifier_temperature # [degC] The temperature of the rectifier or power converter. INT16_MAX: field not provided. +int16 generator_temperature # [degC] The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided. diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 76caf4c732..01df3f9255 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -283,6 +283,8 @@ rtps: id: 134 - msg: estimator_states id: 135 + - msg: generator_status + id: 136 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 2fd1ab79c1..09bf186340 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -63,6 +63,7 @@ void LoggedTopics::add_default_topics() add_topic("estimator_sensor_bias", 1000); add_topic("estimator_states", 1000); add_topic("estimator_status", 200); + add_topic("generator_status"); add_topic("home_position"); add_topic("hover_thrust_estimate", 100); add_topic("input_rc", 500); @@ -79,9 +80,9 @@ void LoggedTopics::add_default_topics() add_topic("safety"); add_topic("sensor_combined"); add_topic("sensor_correction"); - add_topic("sensors_status_imu", 200); add_topic("sensor_preflight_mag", 500); add_topic("sensor_selection"); + add_topic("sensors_status_imu", 200); add_topic("system_power", 500); add_topic("tecs_status", 200); add_topic("test_motor", 500); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0616b481ce..0d1587d349 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -267,6 +267,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_onboard_computer_status(msg); break; + case MAVLINK_MSG_ID_GENERATOR_STATUS: + handle_message_generator_status(msg); + break; + case MAVLINK_MSG_ID_STATUSTEXT: handle_message_statustext(msg); break; @@ -2780,6 +2784,28 @@ MavlinkReceiver::handle_message_onboard_computer_status(mavlink_message_t *msg) _onboard_computer_status_pub.publish(onboard_computer_status_topic); } +void MavlinkReceiver::handle_message_generator_status(mavlink_message_t *msg) +{ + mavlink_generator_status_t status_msg; + mavlink_msg_generator_status_decode(msg, &status_msg); + + generator_status_s generator_status{}; + generator_status.timestamp = hrt_absolute_time(); + generator_status.status = status_msg.status; + generator_status.battery_current = status_msg.battery_current; + generator_status.load_current = status_msg.load_current; + generator_status.power_generated = status_msg.power_generated; + generator_status.bus_voltage = status_msg.bus_voltage; + generator_status.bat_current_setpoint = status_msg.bat_current_setpoint; + generator_status.runtime = status_msg.runtime; + generator_status.time_until_maintenance = status_msg.time_until_maintenance; + generator_status.generator_speed = status_msg.generator_speed; + generator_status.rectifier_temperature = status_msg.rectifier_temperature; + generator_status.generator_temperature = status_msg.generator_temperature; + + _generator_status_pub.publish(generator_status); +} + void MavlinkReceiver::handle_message_statustext(mavlink_message_t *msg) { if (msg->sysid == mavlink_system.sysid) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a3909a05f0..55fc28a1e5 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -148,6 +149,7 @@ private: void handle_message_debug_vect(mavlink_message_t *msg); void handle_message_distance_sensor(mavlink_message_t *msg); void handle_message_follow_target(mavlink_message_t *msg); + void handle_message_generator_status(mavlink_message_t *msg); void handle_message_gps_global_origin(mavlink_message_t *msg); void handle_message_gps_rtcm_data(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); @@ -161,6 +163,7 @@ private: void handle_message_named_value_float(mavlink_message_t *msg); void handle_message_obstacle_distance(mavlink_message_t *msg); void handle_message_odometry(mavlink_message_t *msg); + void handle_message_onboard_computer_status(mavlink_message_t *msg); void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_ping(mavlink_message_t *msg); void handle_message_play_tune(mavlink_message_t *msg); @@ -171,14 +174,13 @@ private: void handle_message_set_actuator_control_target(mavlink_message_t *msg); void handle_message_set_attitude_target(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); - void handle_message_set_position_target_local_ned(mavlink_message_t *msg); void handle_message_set_position_target_global_int(mavlink_message_t *msg); + void handle_message_set_position_target_local_ned(mavlink_message_t *msg); void handle_message_statustext(mavlink_message_t *msg); void handle_message_trajectory_representation_bezier(mavlink_message_t *msg); void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg); void handle_message_utm_global_position(mavlink_message_t *msg); void handle_message_vision_position_estimate(mavlink_message_t *msg); - void handle_message_onboard_computer_status(mavlink_message_t *msg); void Run(); @@ -245,6 +247,7 @@ private: uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; + uORB::Publication _generator_status_pub{ORB_ID(generator_status)}; uORB::Publication _flow_pub{ORB_ID(optical_flow)}; uORB::Publication _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; @@ -258,7 +261,7 @@ private: uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; - uORB::Publication _trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; + uORB::Publication _trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; uORB::Publication _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; // ORB publications (multi)