From cbe75188e93d832ec248883140c8376d0b9edc65 Mon Sep 17 00:00:00 2001 From: acfloria Date: Tue, 30 Jan 2018 18:04:37 +0100 Subject: [PATCH] Add the HighLatency2 mavlink stream --- src/modules/mavlink/mavlink_messages.cpp | 547 ++++++++++++++++++----- src/modules/mavlink/mavlink_messages.h | 32 +- src/modules/mavlink/mavlink_stream.cpp | 2 + src/modules/mavlink/mavlink_stream.h | 5 + 4 files changed, 474 insertions(+), 112 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b1e9ed05ab..5ebedc515c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -94,6 +95,7 @@ #include #include #include +#include #include #include #include @@ -3931,22 +3933,22 @@ protected: } }; -class MavlinkStreamHighLatency : public MavlinkStream +class MavlinkStreamHighLatency2 : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamHighLatency::get_name_static(); + return MavlinkStreamHighLatency2::get_name_static(); } static const char *get_name_static() { - return "HIGH_LATENCY"; + return "HIGH_LATENCY2"; } static uint16_t get_id_static() { - return MAVLINK_MSG_ID_HIGH_LATENCY; + return MAVLINK_MSG_ID_HIGH_LATENCY2; } uint16_t get_id() @@ -3956,17 +3958,20 @@ public: static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamHighLatency(mavlink); + return new MavlinkStreamHighLatency2(mavlink); } unsigned get_size() { - return MAVLINK_MSG_ID_HIGH_LATENCY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return MAVLINK_MSG_ID_HIGH_LATENCY2_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } private: - MavlinkOrbSubscription *_actuator_sub; - uint64_t _actuator_time; + MavlinkOrbSubscription *_actuator_sub_0; + uint64_t _actuator_time_0; + + MavlinkOrbSubscription *_actuator_sub_1; + uint64_t _actuator_time_1; MavlinkOrbSubscription *_airspeed_sub; uint64_t _airspeed_time; @@ -3974,168 +3979,503 @@ private: MavlinkOrbSubscription *_attitude_sp_sub; uint64_t _attitude_sp_time; - MavlinkOrbSubscription *_attitude_sub; - uint64_t _attitude_time; - MavlinkOrbSubscription *_battery_sub; uint64_t _battery_time; + MavlinkOrbSubscription *_estimator_status_sub; + uint64_t _estimator_status_time; + MavlinkOrbSubscription *_fw_pos_ctrl_status_sub; uint64_t _fw_pos_ctrl_status_time; + MavlinkOrbSubscription *_geofence_sub; + uint64_t _geofence_time; + MavlinkOrbSubscription *_global_pos_sub; uint64_t _global_pos_time; MavlinkOrbSubscription *_gps_sub; uint64_t _gps_time; - MavlinkOrbSubscription *_home_sub; - uint64_t _home_time; - - MavlinkOrbSubscription *_landed_sub; - uint64_t _landed_time; - MavlinkOrbSubscription *_mission_result_sub; uint64_t _mission_result_time; - MavlinkOrbSubscription *_sensor_sub; - uint64_t _sensor_time; - MavlinkOrbSubscription *_status_sub; uint64_t _status_time; + MavlinkOrbSubscription *_status_flags_sub; + uint64_t _status_flags_time; + MavlinkOrbSubscription *_tecs_status_sub; uint64_t _tecs_time; MavlinkOrbSubscription *_wind_sub; uint64_t _wind_time; + SimpleAnalyzer _airspeed; + SimpleAnalyzer _airspeed_sp; + SimpleAnalyzer _battery; + SimpleAnalyzer _climb_rate; + SimpleAnalyzer _eph; + SimpleAnalyzer _epv; + SimpleAnalyzer _groundspeed; + SimpleAnalyzer _temperature; + SimpleAnalyzer _throttle; + SimpleAnalyzer _windspeed; + /* do not allow top copying this class */ - MavlinkStreamHighLatency(MavlinkStreamHighLatency &); - MavlinkStreamHighLatency &operator = (const MavlinkStreamHighLatency &); + MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &); + MavlinkStreamHighLatency2 &operator = (const MavlinkStreamHighLatency2 &); protected: - explicit MavlinkStreamHighLatency(Mavlink *mavlink) : MavlinkStream(mavlink), - _actuator_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), - _actuator_time(0), + explicit MavlinkStreamHighLatency2(Mavlink *mavlink) : MavlinkStream(mavlink), + _actuator_sub_0(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), + _actuator_time_0(0), + _actuator_sub_1(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_1))), + _actuator_time_1(0), _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), _airspeed_time(0), _attitude_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))), _attitude_sp_time(0), - _attitude_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), - _attitude_time(0), _battery_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status))), _battery_time(0), + _estimator_status_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))), + _estimator_status_time(0), _fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))), _fw_pos_ctrl_status_time(0), + _geofence_sub(_mavlink->add_orb_subscription(ORB_ID(geofence_result))), + _geofence_time(0), _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), _global_pos_time(0), _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))), _gps_time(0), - _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), - _home_time(0), - _landed_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_land_detected))), - _landed_time(0), _mission_result_sub(_mavlink->add_orb_subscription(ORB_ID(mission_result))), _mission_result_time(0), - _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), - _sensor_time(0), _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), _status_time(0), + _status_flags_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status_flags))), + _status_flags_time(0), _tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status))), - _tecs_time(0) + _tecs_time(0), + _wind_sub(_mavlink->add_orb_subscription(ORB_ID(wind_estimate))), + _wind_time(0), + _airspeed(SimpleAnalyzer::AVERAGE), + _airspeed_sp(SimpleAnalyzer::AVERAGE), + _battery(SimpleAnalyzer::AVERAGE), + _climb_rate(SimpleAnalyzer::MAX), + _eph(SimpleAnalyzer::MAX), + _epv(SimpleAnalyzer::MAX), + _groundspeed(SimpleAnalyzer::AVERAGE), + _temperature(SimpleAnalyzer::AVERAGE), + _throttle(SimpleAnalyzer::AVERAGE), + _windspeed(SimpleAnalyzer::AVERAGE) {} bool send(const hrt_abstime t) { - actuator_controls_s actuator = {}; - airspeed_s airspeed = {}; - battery_status_s battery = {}; - fw_pos_ctrl_status_s fw_pos_ctrl_status = {}; - home_position_s home = {}; - mission_result_s mission_result = {}; - sensor_combined_s sensor = {}; - tecs_status_s tecs_status = {}; - vehicle_attitude_s attitude = {}; - vehicle_attitude_setpoint_s attitude_sp = {}; - vehicle_global_position_s global_pos = {}; - vehicle_gps_position_s gps = {}; - vehicle_land_detected_s land_detected = {}; - vehicle_status_s status = {}; - - bool updated = _status_sub->update(&_status_time, &status); - updated |= _actuator_sub->update(&_actuator_time, &actuator); - updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); + struct airspeed_s airspeed = {}; + struct battery_status_s battery = {}; + struct estimator_status_s estimator_status = {}; + struct fw_pos_ctrl_status_s fw_pos_ctrl_status = {}; + struct mission_result_s mission_result = {}; + struct geofence_result_s geofence = {}; + struct tecs_status_s tecs_status = {}; + struct vehicle_attitude_setpoint_s attitude_sp = {}; + struct vehicle_global_position_s global_pos = {}; + struct vehicle_status_s status = {}; + struct vehicle_status_flags_s status_flags = {}; + struct wind_estimate_s wind = {}; + + bool updated = _airspeed_sub->update(&_airspeed_time, &airspeed); + updated |= _airspeed.valid(); + updated |= _airspeed_sp.valid(); + updated |= _battery.valid(); + updated |= _climb_rate.valid(); + updated |= _eph.valid(); + updated |= _epv.valid(); + updated |= _groundspeed.valid(); + updated |= _temperature.valid(); + updated |= _throttle.valid(); + updated |= _windspeed.valid(); updated |= _attitude_sp_sub->update(&_attitude_sp_time, &attitude_sp); - updated |= _attitude_sub->update(&_attitude_time, &attitude); updated |= _battery_sub->update(&_battery_time, &battery); + updated |= _estimator_status_sub->update(&_estimator_status_time, &estimator_status); updated |= _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status_time, &fw_pos_ctrl_status); + updated |= _geofence_sub->update(&_geofence_time, &geofence); updated |= _global_pos_sub->update(&_global_pos_time, &global_pos); - updated |= _gps_sub->update(&_gps_time, &gps); - updated |= _home_sub->update(&_home_time, &home); - updated |= _landed_sub->update(&_landed_time, &land_detected); updated |= _mission_result_sub->update(&_mission_result_time, &mission_result); - updated |= _sensor_sub->update(&_sensor_time, &sensor); + updated |= _status_sub->update(&_status_time, &status); + updated |= _status_flags_sub->update(&_status_flags_time, &status_flags); updated |= _tecs_status_sub->update(&_tecs_time, &tecs_status); + updated |= _wind_sub->update(&_wind_time, &wind); if (updated) { - mavlink_high_latency_t msg = {}; + mavlink_high_latency2_t msg = {}; + set_default_values(msg); - //timespec tv; - //px4_clock_gettime(CLOCK_REALTIME, &tv); - //msg.time_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; + convert_limit_safe(t / 1000, &msg.timestamp); + msg.type = _mavlink->get_system_type(); + msg.autopilot = MAV_AUTOPILOT_PX4; - msg.base_mode = 0; - msg.custom_mode = 0; - uint8_t sys_status; - get_mavlink_mode_state(&status, &sys_status, &msg.base_mode, &msg.custom_mode); + if (_attitude_sp_time > 0) { + msg.target_heading = static_cast(math::degrees(_wrap_2pi(attitude_sp.yaw_body)) * 0.5f); + } - matrix::Eulerf euler = matrix::Quatf(attitude.q); - msg.roll = math::degrees(euler.phi()) * 100; - msg.pitch = math::degrees(euler.theta()) * 100; - msg.heading = math::degrees(_wrap_2pi(euler.psi())) * 100; + if (_fw_pos_ctrl_status_time > 0) { + convert_limit_safe(fw_pos_ctrl_status.wp_dist * 0.1f, &msg.target_distance); + } - //msg.roll_sp = math::degrees(attitude_sp.roll_body) * 100; - //msg.pitch_sp = math::degrees(attitude_sp.pitch_body) * 100; - msg.heading_sp = math::degrees(_wrap_2pi(attitude_sp.yaw_body)) * 100; + if (_global_pos_time > 0) { + convert_limit_safe(global_pos.lat * 1e7, &msg.latitude); + convert_limit_safe(global_pos.lon * 1e7, &msg.longitude); - if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - msg.throttle = actuator.control[actuator_controls_s::INDEX_THROTTLE] * 100; + if (global_pos.alt > 0) { + convert_limit_safe(global_pos.alt + 0.5f, &msg.altitude); - } else { - msg.throttle = 0; + } else { + convert_limit_safe(global_pos.alt - 0.5f, &msg.altitude); + } + + msg.heading = static_cast(math::degrees(_wrap_2pi(global_pos.yaw)) * 0.5f); } - msg.latitude = global_pos.lat * 1e7; - msg.longitude = global_pos.lon * 1e7; + if (_mission_result_time > 0) { + msg.wp_num = mission_result.seq_current; + } - //msg.altitude_home = (_home_time > 0) ? (global_pos.alt - home.alt) : NAN; - msg.altitude_amsl = (_global_pos_time > 0) ? global_pos.alt : NAN; + if (_tecs_time > 0) { + convert_limit_safe(tecs_status.altitudeSp, &msg.target_altitude); + } - msg.altitude_sp = ((_tecs_time > 0) && home.valid_alt) ? (tecs_status.altitudeSp - home.alt) : NAN; + if (_wind_time > 0) { + msg.wind_heading = static_cast( + math::degrees(_wrap_2pi(atan2f(wind.windspeed_east, wind.windspeed_north))) * 0.5f); + } + + if (_airspeed.valid()) { + _airspeed.get_scaled(msg.airspeed, 5.0f); + } + + if (_airspeed_sp.valid()) { + _airspeed_sp.get_scaled(msg.airspeed_sp, 5.0f); + } - msg.airspeed = airspeed.indicated_airspeed_m_s * 100.0f; - msg.groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) * 100.0f; - msg.climb_rate = -global_pos.vel_d; + if (_battery.valid()) { + _battery.get_scaled(msg.battery, 100.0f); + } - msg.gps_nsat = gps.satellites_used; - msg.gps_fix_type = gps.fix_type; + if (_climb_rate.valid()) { + _climb_rate.get_scaled(msg.climb_rate, 10.0f); + } - msg.landed_state = land_detected.landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR; + if (_eph.valid()) { + _eph.get_scaled(msg.eph, 10.0f); + } - msg.battery_remaining = (battery.connected) ? battery.remaining * 100.0f : -1; + if (_epv.valid()) { + _epv.get_scaled(msg.epv, 10.0f); + } - msg.temperature = sensor.baro_temp_celcius; - msg.temperature_air = airspeed.air_temperature_celsius; + if (_groundspeed.valid()) { + _groundspeed.get_scaled(msg.groundspeed, 5.0f); + } + + if (_temperature.valid()) { + _temperature.get_scaled(msg.temperature_air, 1.0f); + } + + if (_throttle.valid()) { + _throttle.get_scaled(msg.throttle, 100.0f); + } + + if (_windspeed.valid()) { + _windspeed.get_scaled(msg.windspeed, 5.0f); + } + + // failsafe flags, requires an initial value of 0 + if (_battery_time > 0) { + if (battery.warning > battery_status_s::BATTERY_WARNING_LOW) { + msg.failsafe |= MAV_FAILSAFE_FLAG_BATTERY; + } + } + + if (_status_time > 0) { + if (status.rc_signal_lost) { + msg.failsafe |= MAV_FAILSAFE_FLAG_RC_LOSS; + } + + if (status.engine_failure) { + msg.failsafe |= MAV_FAILSAFE_FLAG_ENGINE; + } + + if (status.data_link_lost) { + msg.failsafe |= MAV_FAILSAFE_FLAG_TELEM_LOSS; + } + } + + if (_geofence_time > 0) { + if (geofence.geofence_violated) { + msg.failsafe |= MAV_FAILSAFE_FLAG_GEOFENCE; + } + } + + if (_status_flags_time > 0) { + if (status_flags.gps_failure || !status_flags.condition_global_position_valid) { + msg.failsafe |= MAV_FAILSAFE_FLAG_GPS; + } + } + + // failure flags + + if (_airspeed_time > 0) { + if (airspeed.confidence < 0.95f) { // the same threshold as for the commander + msg.failure_flags |= MAV_FAILURE_FLAG_AIRSPEED; + } + } + + if (_estimator_status_time > 0) { + if (estimator_status.gps_check_fail_flags > 0 || + estimator_status.filter_fault_flags > 0 || + estimator_status.innovation_check_flags > 0) { + msg.failure_flags |= MAV_FAILURE_FLAG_ESTIMATOR; + } + } + + if (_status_flags_time > 0) { + if (status_flags.gps_failure) { + msg.failure_flags |= MAV_FAILURE_FLAG_GPS; + } + + if (status_flags.offboard_control_signal_lost && status_flags.offboard_control_signal_found_once) { + msg.failure_flags |= MAV_FAILURE_FLAG_OFFBOARD_LINK; + } + } + + if (_status_time > 0) { + if (((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_ACCEL) + && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_ACCEL)) || + ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_ACCEL2) && + !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_ACCEL2))) { + msg.failure_flags |= MAV_FAILURE_FLAG_ACCELEROMETER; + } + + if ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE) + && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE)) { + msg.failure_flags |= MAV_FAILURE_FLAG_BAROMETER; + } + + if (((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_GYRO) + && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_GYRO)) || + ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_GYRO2) && + !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_GYRO2))) { + msg.failure_flags |= MAV_FAILURE_FLAG_GYROSCOPE; + } + + if ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_LASER_POSITION) + && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_LASER_POSITION)) { + msg.failure_flags |= MAV_FAILURE_FLAG_LIDAR; + } + + if (((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_MAG) + && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_MAG)) || + ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_MAG2) && + !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_MAG2))) { + msg.failure_flags |= MAV_FAILURE_FLAG_MAGNETOMETER; + } + + if (status.mission_failure) { + msg.failure_flags |= MAV_FAILURE_FLAG_MISSION; + } + } + + // flight mode + switch (status.nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: + msg.flight_mode = FLIGHT_MODE_MANUAL; + break; + + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: + msg.flight_mode = FLIGHT_MODE_ALTCTL; + break; + + case vehicle_status_s::NAVIGATION_STATE_POSCTL: + msg.flight_mode = FLIGHT_MODE_POSCTL; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + msg.flight_mode = FLIGHT_MODE_AUTO_MISSION; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: + msg.flight_mode = FLIGHT_MODE_AUTO_LOITER; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + msg.flight_mode = FLIGHT_MODE_AUTO_RTL; + break; + + case vehicle_status_s::NAVIGATION_STATE_ACRO: + msg.flight_mode = FLIGHT_MODE_ACRO; + break; + + case vehicle_status_s::NAVIGATION_STATE_DESCEND: + msg.flight_mode = FLIGHT_MODE_DESCEND; + break; + + case vehicle_status_s::NAVIGATION_STATE_TERMINATION: + msg.flight_mode = FLIGHT_MODE_TERMINATION; + break; + + case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: + msg.flight_mode = FLIGHT_MODE_OFFBOARD; + break; + + case vehicle_status_s::NAVIGATION_STATE_STAB: + msg.flight_mode = FLIGHT_MODE_STABILIZED; + break; + + case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: + msg.flight_mode = FLIGHT_MODE_RATTITIDE; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + msg.flight_mode = FLIGHT_MODE_AUTO_TAKEOFF; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: + msg.flight_mode = FLIGHT_MODE_AUTO_LAND; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: + msg.flight_mode = FLIGHT_MODE_AUTO_FOLLOW_TARGET; + break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND: + msg.flight_mode = FLIGHT_MODE_AUTO_PRECLAND; + break; + + default: + msg.flight_mode = FLIGHT_MODE_ENUM_END; + } - msg.wp_num = mission_result.seq_current; - msg.wp_distance = fw_pos_ctrl_status.wp_dist; - mavlink_msg_high_latency_send_struct(_mavlink->get_channel(), &msg); + + // reset the analyzers + _airspeed.reset(); + _airspeed_sp.reset(); + _battery.reset(); + _climb_rate.reset(); + _eph.reset(); + _epv.reset(); + _groundspeed.reset(); + _temperature.reset(); + _throttle.reset(); + _windspeed.reset(); + + mavlink_msg_high_latency2_send_struct(_mavlink->get_channel(), &msg); } return updated; } + + void update_data() + { + const float update_rate = 1.0f / (_mavlink->get_main_loop_delay() * 1e-6f); + + struct airspeed_s airspeed = {}; + + if (_airspeed_sub->update(&airspeed)) { + _airspeed.add_value(airspeed.indicated_airspeed_m_s, update_rate); + _temperature.add_value(airspeed.air_temperature_celsius, update_rate); + } + + struct tecs_status_s tecs_status = {}; + + if (_tecs_status_sub->update(&tecs_status)) { + _airspeed_sp.add_value(tecs_status.airspeedSp, update_rate); + } + + struct battery_status_s battery = {}; + + if (_battery_sub->update(&battery)) { + _battery.add_value(battery.remaining, update_rate); + } + + struct vehicle_global_position_s global_pos = {}; + + if (_global_pos_sub->update(&global_pos)) { + _climb_rate.add_value(fabs(global_pos.vel_d), update_rate); + _groundspeed.add_value(sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e), update_rate); + } + + struct vehicle_gps_position_s gps = {}; + + if (_gps_sub->update(&gps)) { + _eph.add_value(gps.eph, update_rate); + _epv.add_value(gps.epv, update_rate); + } + + struct vehicle_status_s status = {}; + + if (_status_sub->update(&status)) { + if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + struct actuator_controls_s actuator = {}; + + if (status.is_vtol && !status.is_rotary_wing) { + if (_actuator_sub_1->update(&actuator)) { + _throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], update_rate); + } + + } else { + if (_actuator_sub_0->update(&actuator)) { + _throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], update_rate); + } + } + + } else { + _throttle.add_value(0.0f, update_rate); + } + } + + struct wind_estimate_s wind = {}; + + if (_wind_sub->update(&wind)) { + _windspeed.add_value(sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east), + update_rate); + } + } + + void set_default_values(mavlink_high_latency2_t &msg) const + { + msg.airspeed = 0; + msg.airspeed_sp = 0; + msg.altitude = 0; + msg.autopilot = MAV_AUTOPILOT_ENUM_END; + msg.battery = -1; + msg.climb_rate = 0; + msg.custom0 = INT8_MIN; + msg.custom1 = INT8_MIN; + msg.custom2 = INT8_MIN; + msg.eph = UINT8_MAX; + msg.epv = UINT8_MAX; + msg.failsafe = 0; + msg.failure_flags = 0; + msg.flight_mode = FLIGHT_MODE_ENUM_END; + msg.groundspeed = 0; + msg.heading = 0; + msg.latitude = 0; + msg.longitude = 0; + msg.target_altitude = 0; + msg.target_distance = 0; + msg.target_heading = 0; + msg.temperature_air = INT8_MIN; + msg.throttle = 0; + msg.timestamp = 0; + msg.type = MAV_TYPE_ENUM_END; + msg.wind_heading = 0; + msg.windspeed = 0; + msg.wp_num = UINT16_MAX; + } }; @@ -4283,7 +4623,7 @@ static const StreamListItem streams_list[] = { StreamListItem(&MavlinkStreamCollision::new_instance, &MavlinkStreamCollision::get_name_static, &MavlinkStreamCollision::get_id_static), StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static), StreamListItem(&MavlinkStreamMountOrientation::new_instance, &MavlinkStreamMountOrientation::get_name_static, &MavlinkStreamMountOrientation::get_id_static), - StreamListItem(&MavlinkStreamHighLatency::new_instance, &MavlinkStreamHighLatency::get_name_static, &MavlinkStreamHighLatency::get_id_static), + StreamListItem(&MavlinkStreamHighLatency2::new_instance, &MavlinkStreamHighLatency2::get_name_static, &MavlinkStreamHighLatency2::get_id_static), StreamListItem(&MavlinkStreamGroundTruth::new_instance, &MavlinkStreamGroundTruth::get_name_static, &MavlinkStreamGroundTruth::get_id_static) }; @@ -4313,8 +4653,8 @@ MavlinkStream *create_mavlink_stream(const char *stream_name, Mavlink *mavlink) return nullptr; } -SimpleAnalyzer::SimpleAnalyzer(Mode mode, unsigned int n_max) : - _n_max(n_max), +SimpleAnalyzer::SimpleAnalyzer(Mode mode, float window) : + _window(window), _mode(mode) { reset(); @@ -4343,12 +4683,13 @@ void SimpleAnalyzer::reset() _result = FLT_MIN; break; + default: PX4_ERR("SimpleAnalyzer: Unknown mode."); } } -void SimpleAnalyzer::add_value(float val) +void SimpleAnalyzer::add_value(float val, float update_rate) { switch (_mode) { case AVERAGE: @@ -4373,7 +4714,7 @@ void SimpleAnalyzer::add_value(float val) // if we get more measurements than n_max so the exponential moving average // is computed - if (_n < _n_max) { + if ((_n < update_rate * _window) && (update_rate > 1.0f)) { _n++; } @@ -4384,22 +4725,22 @@ void SimpleAnalyzer::add_value(float val) } } -bool SimpleAnalyzer::valid() +bool SimpleAnalyzer::valid() const { return _n > 0u; } -float SimpleAnalyzer::get() +float SimpleAnalyzer::get() const { return _result; } -float SimpleAnalyzer::get_scaled(float scalingfactor) +float SimpleAnalyzer::get_scaled(float scalingfactor) const { return get() * scalingfactor; } -void SimpleAnalyzer::check_limits(float &x, float min, float max) +void SimpleAnalyzer::check_limits(float &x, float min, float max) const { if (x > max) { x = max; @@ -4409,7 +4750,7 @@ void SimpleAnalyzer::check_limits(float &x, float min, float max) } } -void SimpleAnalyzer::int_round(float &x) +void SimpleAnalyzer::int_round(float &x) const { if (x < 0) { x -= 0.5f; diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 0aa8389acc..e02778ee93 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -72,17 +72,17 @@ public: MAX, }; - SimpleAnalyzer(Mode mode, unsigned int n_max = 1000); + SimpleAnalyzer(Mode mode, float window = 60.0f); virtual ~SimpleAnalyzer(); void reset(); - void add_value(float val); - bool valid(); - float get(); - float get_scaled(float scalingfactor); + void add_value(float val, float update_rate); + bool valid() const; + float get() const; + float get_scaled(float scalingfactor) const; template - void get_scaled(T &ret, float scalingfactor) + void get_scaled(T &ret, float scalingfactor) const { float avg = get_scaled(scalingfactor); int_round(avg); @@ -93,12 +93,26 @@ public: private: unsigned int _n = 0; - unsigned int _n_max = 1000; + float _window = 60.0f; Mode _mode = AVERAGE; float _result = 0.0f; - void check_limits(float &x, float min, float max); - void int_round(float &x); + void check_limits(float &x, float min, float max) const; + void int_round(float &x) const; }; +template +void convert_limit_safe(Tin in, Tout *out) +{ + if (in > std::numeric_limits::max()) { + *out = std::numeric_limits::max(); + + } else if (in < std::numeric_limits::min()) { + *out = std::numeric_limits::min(); + + } else { + *out = static_cast(in); + } +} + #endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 54b9dd7bc8..c16b2ff254 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -71,6 +71,8 @@ MavlinkStream::set_interval(const int interval) int MavlinkStream::update(const hrt_abstime t) { + update_data(); + // If the message has never been sent before we want // to send it immediately and can return right away if (_last_sent == 0) { diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index be87946e70..af05cf42e7 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -103,6 +103,11 @@ protected: virtual bool send(const hrt_abstime t) = 0; #endif + /** + * Function to collect/update data for the streams when update() is called. + */ + virtual void update_data() { } + private: hrt_abstime _last_sent;