diff --git a/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp b/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp index bd0387237d..3919fb19c0 100644 --- a/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp +++ b/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp @@ -66,6 +66,8 @@ #include #include +#include + using namespace time_literals; /* Configuration Constants */ @@ -177,6 +179,8 @@ private: (ParamInt) _p_sensor10_rot, (ParamInt) _p_sensor11_rot ); + orb_advert_t _mavlink_log_pub{nullptr}; ///< mavlink log pub + }; TFMINI_I2C::TFMINI_I2C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) : @@ -280,6 +284,9 @@ TFMINI_I2C::collect() report.timestamp = hrt_absolute_time(); report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; report.variance = 0.0f; + report.h_fov = math::radians(1.15f); // 50.0f, 0.0488692f + report.v_fov = math::radians(1.15f); + int instance_id; orb_publish_auto(ORB_ID(distance_sensor), &_distance_sensor_topic, &report, &instance_id); @@ -293,6 +300,16 @@ TFMINI_I2C::collect() } perf_end(_sample_perf); + + // static uint32_t last_2s,delt_time; + // uint32_t nowtime = hrt_absolute_time(); + // if( (nowtime - last_2s) > 2_s) + // { + // mavlink_log_info(&_mavlink_log_pub, "dist(%d) : %.2f,%.2f\n",(nowtime-delt_time),(double)distance_m,(double)report.h_fov); + // last_2s = nowtime; + // } + // delt_time = hrt_absolute_time(); + return PX4_OK; } diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 6a4cfb723c..625d55e96e 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -529,6 +529,16 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max _constraints_pub.publish(constraints); original_setpoint = new_setpoint; + + static uint32_t last_2s,delt_time; + uint32_t nowtime = getTime(); + if( (nowtime - last_2s) > 2_s) + { + printf( "Obstacle step(%d): %.2f,%.2f\n",delt_time,(double)original_setpoint(0),(double)original_setpoint(1)); + last_2s = nowtime; + } + delt_time = getTime(); + } void CollisionPrevention::_publishVehicleCmdDoLoiter() diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 1cf60e82ba..1017366a08 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -728,15 +728,20 @@ Commander::handle_command(const vehicle_command_s &cmd) if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, _internal_state); + mavlink_log_info(&_mavlink_log_pub, "cmd mode:%d,ret:%d\n",custom_main_mode,main_ret); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, _internal_state); + mavlink_log_info(&_mavlink_log_pub, "cmd mode:%d,ret:%d\n",custom_main_mode,main_ret); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ reset_posvel_validity(); main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state); + mavlink_log_info(&_mavlink_log_pub, "cmd mode:%d,ret:%d,lo:%d,go:%d\n",custom_main_mode,main_ret,_status_flags.condition_local_altitude_valid,_status_flags.condition_global_position_valid); + main_ret = TRANSITION_CHANGED; + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 48f914a448..7d08e11044 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -41,6 +41,8 @@ #include #include +#include + using namespace matrix; FlightTaskManualAltitude::FlightTaskManualAltitude() : @@ -376,14 +378,15 @@ bool FlightTaskManualAltitude::update() _scaleSticks(); _updateSetpoints(); _constraints.want_takeoff = _checkTakeoff(); - - static uint64_t delay_1s = hrt_absolute_time(); - static uint64_t delt_time; - if (hrt_absolute_time() - delay_1s > 1 * 1000000ULL) + static uint32_t last_2s,delt_time; + uint32_t nowtime = hrt_absolute_time(); + if( (nowtime - last_2s) > 2 * 1000000ULL) { - printf("update,delt:%dms \n",(hrt_absolute_time() - delt_time)/(1 * 1000ULL) ); - delay_1s = hrt_absolute_time(); + mavlink_log_info(&_mavlink_log_pub, "FlightTaskManualAltitude(%d)\n",(nowtime - delt_time)/1000U); + printf( "FlightTaskManualAltitude(%d)\n",(nowtime - delt_time)/1000U ); + last_2s = nowtime; } - delt_time = hrt_absolute_time(); + delt_time = hrt_absolute_time(); + return ret; } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index c4b14200a0..20e8a4b813 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -151,4 +151,5 @@ private: float _dist_to_ground_lock = NAN; AlphaFilter _man_input_filter; + orb_advert_t _mavlink_log_pub{nullptr}; }; diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index 5ef59be4bb..06daff9b3c 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -39,6 +39,7 @@ #include #include +#include using namespace matrix; FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(this) @@ -114,6 +115,16 @@ void FlightTaskManualPosition::_scaleSticks() } _velocity_setpoint.xy() = vel_sp_xy; + + + static uint32_t last_2s,delt_time; + uint32_t nowtime = hrt_absolute_time(); + if( (nowtime - last_2s) > 2_s) + { + mavlink_log_info(&_mavlink_log_pub, "scaleSticks(%d) : %.2f,%.2f\n",delt_time,(double)vel_sp_xy(0),(double)vel_sp_xy(1)); + last_2s = nowtime; + } + delt_time = hrt_absolute_time(); } void FlightTaskManualPosition::_updateXYlock() diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp index b0db7c3387..a36c875202 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -76,4 +76,6 @@ private: nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ CollisionPrevention _collision_prevention; /**< collision avoidance setpoint amendment */ + orb_advert_t _mavlink_log_pub{nullptr}; ///< mavlink log pub + }; diff --git a/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp b/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp index 9042403a48..01ba9e87dc 100644 --- a/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp +++ b/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp @@ -94,6 +94,8 @@ private: msg.min_distance = dist_sensor.min_distance * 1e2f; // m to cm msg.orientation = dist_sensor.orientation; msg.covariance = dist_sensor.variance * 1e4f; // m^2 to cm^2 + msg.horizontal_fov = dist_sensor.h_fov; // m^2 to cm^2 + msg.vertical_fov = dist_sensor.v_fov; // m^2 to cm^2 mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg); diff --git a/v5_build.sh b/v5_build.sh index 5f4ac9a68d..d8c66d4444 100755 --- a/v5_build.sh +++ b/v5_build.sh @@ -1,6 +1,6 @@ date -R starttime=`date +'%Y-%m-%d %H:%M:%S'` -make px4_fmu-v5_default +make px4_fmu-v5_default upload endtime=`date +'%Y-%m-%d %H:%M:%S'` date -R start_seconds=$(date --date="$starttime" +%s);