From 263b00b65fc89c7fd9383f0364760fceb15ea12a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 5 Mar 2021 18:25:14 -0500 Subject: [PATCH] ekf2 support SET_GPS_GLOBAL_ORIGIN and remove globallocalconverter usage - vehicle_command cmd extended from uint16 to support PX4 internal commands that don't map to mavlink --- msg/vehicle_command.msg | 8 ++- msg/vehicle_command_ack.msg | 2 +- src/modules/commander/Commander.cpp | 49 ++++++++++---- src/modules/ekf2/EKF2.cpp | 24 ++++++- src/modules/ekf2/EKF2.hpp | 2 + .../tasks/FlightTask/FlightTask.cpp | 11 +++- .../tasks/FlightTask/FlightTask.hpp | 9 +++ .../tasks/Orbit/FlightTaskOrbit.cpp | 29 ++++----- .../BlockLocalPositionEstimator.cpp | 18 ++++- .../BlockLocalPositionEstimator.hpp | 7 +- .../sensors/mocap.cpp | 10 +-- .../sensors/vision.cpp | 10 +-- .../mavlink/mavlink_command_sender.cpp | 7 +- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 65 ++++++++++--------- src/modules/mavlink/mavlink_receiver.h | 17 ++--- .../RoverPositionControl.cpp | 21 ++++-- .../RoverPositionControl.hpp | 3 + src/modules/simulator/simulator.h | 10 +-- src/modules/simulator/simulator_mavlink.cpp | 24 +++---- 21 files changed, 212 insertions(+), 118 deletions(-) diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 052c5b5979..089934f103 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -87,6 +87,12 @@ uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006 # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location. + +# PX4 vehicle commands (beyond 16 bit mavlink commands) +uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # start of PX4 internal only vehicle commands (> UINT16_MAX) +uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000 # Sets the GPS co-ordinates of the vehicle local origin (0,0,0) position. |Empty|Empty|Empty|Empty|Latitude|Longitude|Altitude| + + uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED | uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED | uint8 VEHICLE_CMD_RESULT_DENIED = 2 # Command PERMANENTLY DENIED | @@ -148,7 +154,7 @@ float32 param4 # Parameter 4, as defined by MAVLink uint16 VEHICLE_CMD enum. float64 param5 # Parameter 5, as defined by MAVLink uint16 VEHICLE_CMD enum. float64 param6 # Parameter 6, as defined by MAVLink uint16 VEHICLE_CMD enum. float32 param7 # Parameter 7, as defined by MAVLink uint16 VEHICLE_CMD enum. -uint16 command # Command ID, as defined MAVLink by uint16 VEHICLE_CMD enum. +uint32 command # Command ID uint8 target_system # System which should execute the command uint8 target_component # Component which should execute the command, 0 for all components uint8 source_system # System sending the command diff --git a/msg/vehicle_command_ack.msg b/msg/vehicle_command_ack.msg index a1861c3473..9f52a8d62b 100644 --- a/msg/vehicle_command_ack.msg +++ b/msg/vehicle_command_ack.msg @@ -15,7 +15,7 @@ uint16 ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5 uint8 ORB_QUEUE_LENGTH = 4 -uint16 command +uint32 command uint8 result bool from_external uint8 result_param1 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f461139a8d..8affad21d5 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -132,8 +132,9 @@ static int power_button_state_notification_cb(board_power_button_state_notificat #endif // BOARD_HAS_POWER_CONTROL #ifndef CONSTRAINED_FLASH -static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 = NAN, float param3 = NAN, - float param4 = NAN, float param5 = NAN, float param6 = NAN, float param7 = NAN) +static bool send_vehicle_command(const uint32_t cmd, const float param1 = NAN, const float param2 = NAN, + const float param3 = NAN, const float param4 = NAN, const double param5 = static_cast(NAN), + const double param6 = static_cast(NAN), const float param7 = NAN) { vehicle_command_s vcmd{}; @@ -174,7 +175,7 @@ int Commander::custom_command(int argc, char *argv[]) if (argc > 1) { if (!strcmp(argv[1], "gyro")) { // gyro calibration: param1 = 1 - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 1.f, 0.f, 0.f, 0.f, 0.0, 0.0, 0.f); } else if (!strcmp(argv[1], "mag")) { if (argc > 2 && (strcmp(argv[2], "quick") == 0)) { @@ -183,30 +184,30 @@ int Commander::custom_command(int argc, char *argv[]) } else { // magnetometer calibration: param2 = 1 - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.0, 0.0, 0.f); } } else if (!strcmp(argv[1], "accel")) { if (argc > 2 && (strcmp(argv[2], "quick") == 0)) { // accelerometer quick calibration: param5 = 3 - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 4.f, 0.f, 0.f); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 4.0, 0.0, 0.f); } else { // accelerometer calibration: param5 = 1 - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 1.0, 0.0, 0.f); } } else if (!strcmp(argv[1], "level")) { // board level calibration: param5 = 2 - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 2.f, 0.f, 0.f); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 2.0, 0.0, 0.f); } else if (!strcmp(argv[1], "airspeed")) { // airspeed calibration: param6 = 2 - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.f, 2.f, 0.f); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.0, 2.0, 0.f); } else if (!strcmp(argv[1], "esc")) { // ESC calibration: param7 = 1 - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 1.f); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.0, 0.0, 1.f); } else { PX4_ERR("argument %s unsupported.", argv[1]); @@ -359,6 +360,25 @@ int Commander::custom_command(int argc, char *argv[]) return (ret ? 0 : 1); } + if (!strcmp(argv[0], "set_ekf_origin")) { + if (argc > 3) { + + double latitude = atof(argv[1]); + double longitude = atof(argv[2]); + float altitude = atof(argv[3]); + + // Set the ekf NED origin global coordinates. + bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN, + 0.f, 0.f, 0.0, 0.0, latitude, longitude, altitude); + return (ret ? 0 : 1); + + } else { + PX4_ERR("missing argument"); + return 0; + } + } + + #endif return print_usage("unknown command"); @@ -1137,9 +1157,9 @@ Commander::handle_command(const vehicle_command_s &cmd) } else { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); - // parameter 1: Yaw in degrees - // parameter 3: Latitude - // parameter 4: Longitude + // parameter 1: Heading (degrees) + // parameter 3: Latitude (degrees) + // parameter 4: Longitude (degrees) // assume vehicle pointing north (0 degrees) if heading isn't specified const float heading_radians = PX4_ISFINITE(cmd.param1) ? math::radians(roundf(cmd.param1)) : 0.f; @@ -1223,6 +1243,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET: case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE: case vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE: + case vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN: /* ignore commands that are handled by other parts of the system */ break; @@ -1255,6 +1276,7 @@ Commander::handle_command_motor_test(const vehicle_command_s &cmd) test_motor_s test_motor{}; test_motor.timestamp = hrt_absolute_time(); test_motor.motor_number = (int)(cmd.param1 + 0.5f) - 1; + int throttle_type = (int)(cmd.param2 + 0.5f); if (throttle_type != 0) { // 0: MOTOR_TEST_THROTTLE_PERCENT @@ -4041,6 +4063,9 @@ The commander module contains the state machine for mode switching and failsafe "Flight mode", false); PRINT_MODULE_USAGE_COMMAND("lockdown"); PRINT_MODULE_USAGE_ARG("off", "Turn lockdown off", true); + PRINT_MODULE_USAGE_COMMAND("set_ekf_origin"); + PRINT_MODULE_USAGE_ARG("lat, lon, alt", "Origin Latitude, Longitude, Altitude", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("lat|lon|alt", "Origin latitude longitude altitude"); #endif PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 73a994fd29..0eaebc4255 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -265,6 +265,28 @@ void EKF2::Run() } } + if (_vehicle_command_sub.updated()) { + vehicle_command_s vehicle_command; + + if (_vehicle_command_sub.update(&vehicle_command)) { + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) { + if (!_ekf.control_status_flags().in_air) { + + uint64_t origin_time {}; + double latitude = vehicle_command.param5; + double longitude = vehicle_command.param6; + float altitude = vehicle_command.param7; + + _ekf.setEkfGlobalOrigin(latitude, longitude, altitude); + + // Validate the ekf origin status. + _ekf.getEkfGlobalOrigin(origin_time, latitude, longitude, altitude); + PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast(altitude)); + } + } + } + } + bool imu_updated = false; imuSample imu_sample_new {}; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 1952630a09..bbf6a49059 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -77,6 +77,7 @@ #include #include #include +#include #include #include #include @@ -219,6 +220,7 @@ private: uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index 621bbb2b7d..a6af526492 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -148,8 +148,15 @@ void FlightTask::_evaluateVehicleLocalPosition() // global frame reference coordinates to enable conversions if (_sub_vehicle_local_position.get().xy_global && _sub_vehicle_local_position.get().z_global) { - globallocalconverter_init(_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon, - _sub_vehicle_local_position.get().ref_alt, _sub_vehicle_local_position.get().ref_timestamp); + if (!map_projection_initialized(&_global_local_proj_ref) + || (_global_local_proj_ref.timestamp != _sub_vehicle_local_position.get().ref_timestamp)) { + + map_projection_init_timestamped(&_global_local_proj_ref, + _sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon, + _sub_vehicle_local_position.get().ref_timestamp); + + _global_local_alt0 = _sub_vehicle_local_position.get().ref_alt; + } } } } diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 827ea71dc6..53ed01ebcc 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -53,6 +53,7 @@ #include #include #include +#include #include struct ekf_reset_counters_s { @@ -212,10 +213,15 @@ protected: virtual void _ekfResetHandlerVelocityZ() {}; virtual void _ekfResetHandlerHeading(float delta_psi) {}; + map_projection_reference_s _global_local_proj_ref{}; + float _global_local_alt0{NAN}; + /* Time abstraction */ static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */ + float _time{}; /**< passed time in seconds since the task was activated */ float _deltatime{}; /**< passed time in seconds since the task was last updated */ + hrt_abstime _time_stamp_activate{}; /**< time stamp when task was activated */ hrt_abstime _time_stamp_current{}; /**< time stamp at the beginning of the current task update */ hrt_abstime _time_stamp_last{}; /**< time stamp when task was last updated */ @@ -223,6 +229,7 @@ protected: /* Current vehicle state */ matrix::Vector3f _position; /**< current vehicle position */ matrix::Vector3f _velocity; /**< current vehicle velocity */ + float _yaw{}; /**< current vehicle yaw heading */ float _dist_to_bottom{}; /**< current height above ground level */ float _dist_to_ground{}; /**< equals _dist_to_bottom if valid, height above home otherwise */ @@ -239,8 +246,10 @@ protected: matrix::Vector3f _velocity_setpoint; matrix::Vector3f _acceleration_setpoint; matrix::Vector3f _jerk_setpoint; + float _yaw_setpoint{}; float _yawspeed_setpoint{}; + matrix::Vector3f _velocity_setpoint_feedback; matrix::Vector3f _acceleration_setpoint_feedback; diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 6c20a85b26..4804df9d01 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -76,22 +76,15 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) // save current yaw estimate for ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING _initial_heading = _yaw; - // TODO: apply x,y / z independently in geo library // commanded center coordinates - // if(PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) { - // map_projection_global_project(command.param5, command.param6, &_center(0), &_center(1)); - // } + if (map_projection_initialized(&_global_local_proj_ref)) { + if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) { + map_projection_global_project(command.param5, command.param6, &_center(0), &_center(1)); + } - // commanded altitude - // if(PX4_ISFINITE(command.param7)) { - // _position_setpoint(2) = gl_ref.alt - command.param7; - // } - - if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6) && PX4_ISFINITE(command.param7)) { - if (globallocalconverter_tolocal(command.param5, command.param6, command.param7, &_center(0), &_center(1), - &_position_setpoint(2))) { - // global to local conversion failed - ret = false; + // commanded altitude + if (PX4_ISFINITE(command.param7)) { + _position_setpoint(2) = _global_local_alt0 - command.param7; } } @@ -109,8 +102,12 @@ bool FlightTaskOrbit::sendTelemetry() orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL orbit_status.yaw_behaviour = _yaw_behaviour; - if (globallocalconverter_toglobal(_center(0), _center(1), _position_setpoint(2), &orbit_status.x, &orbit_status.y, - &orbit_status.z)) { + if (map_projection_initialized(&_global_local_proj_ref)) { + // local -> global + map_projection_reproject(&_global_local_proj_ref, _center(0), _center(1), &orbit_status.x, &orbit_status.y); + orbit_status.z = _global_local_alt0 - _position_setpoint(2); + + } else { return false; // don't send the message if the transformation failed } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 0c783dba60..7ee630e29c 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -106,7 +106,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : // local to global coversion related variables _is_global_cov_init(false), - _global_ref_timestamp(0.0), _ref_lat(0.0), _ref_lon(0.0), _ref_alt(0.0) @@ -168,6 +167,23 @@ void BlockLocalPositionEstimator::Run() return; } + if (_vehicle_command_sub.updated()) { + vehicle_command_s vehicle_command; + + if (_vehicle_command_sub.update(&vehicle_command)) { + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) { + const double latitude = vehicle_command.param5; + const double longitude = vehicle_command.param6; + const float altitude = vehicle_command.param7; + + map_projection_init_timestamped(&_global_local_proj_ref, latitude, longitude, vehicle_command.timestamp); + _global_local_alt0 = altitude; + + PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast(altitude)); + } + } + } + sensor_combined_s imu; if (!_sensors_sub.update(&imu)) { diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 0302e5ec65..e238b0ccb6 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -12,6 +12,7 @@ // uORB Subscriptions #include #include +#include #include #include #include @@ -264,7 +265,7 @@ private: uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::SubscriptionData _sub_armed{ORB_ID(actuator_armed)}; uORB::SubscriptionData _sub_land{ORB_ID(vehicle_land_detected)}; uORB::SubscriptionData _sub_att{ORB_ID(vehicle_attitude)}; @@ -295,6 +296,9 @@ private: // map projection struct map_projection_reference_s _map_ref; + map_projection_reference_s _global_local_proj_ref{}; + float _global_local_alt0{NAN}; + // target mode paramters from landing_target_estimator module enum TargetMode { Target_Moving = 0, @@ -379,7 +383,6 @@ private: // local to global coversion related variables bool _is_global_cov_init; - uint64_t _global_ref_timestamp; double _ref_lat; double _ref_lon; float _ref_alt; diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp index 192350fd45..c62098aa6f 100644 --- a/src/modules/local_position_estimator/sensors/mocap.cpp +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -37,9 +37,11 @@ void BlockLocalPositionEstimator::mocapInit() _sensorFault &= ~SENSOR_MOCAP; // get reference for global position - globallocalconverter_getref(&_ref_lat, &_ref_lon, &_ref_alt); - _global_ref_timestamp = _timeStamp; - _is_global_cov_init = globallocalconverter_initialized(); + _ref_lat = math::degrees(_global_local_proj_ref.lat_rad); + _ref_lon = math::degrees(_global_local_proj_ref.lon_rad); + _ref_alt = _global_local_alt0; + + _is_global_cov_init = map_projection_initialized(&_global_local_proj_ref); if (!_map_ref.init_done && _is_global_cov_init && !_visionUpdated) { // initialize global origin using the mocap estimator reference (only if the vision estimation is not being fused as well) @@ -53,7 +55,7 @@ void BlockLocalPositionEstimator::mocapInit() if (!_altOriginInitialized) { _altOriginInitialized = true; _altOriginGlobal = true; - _altOrigin = globallocalconverter_initialized() ? _ref_alt : 0.0f; + _altOrigin = map_projection_initialized(&_global_local_proj_ref) ? _ref_alt : 0.0f; } } } diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index 35b8a261ad..9c0dfffef9 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -42,9 +42,11 @@ void BlockLocalPositionEstimator::visionInit() _sensorFault &= ~SENSOR_VISION; // get reference for global position - globallocalconverter_getref(&_ref_lat, &_ref_lon, &_ref_alt); - _global_ref_timestamp = _timeStamp; - _is_global_cov_init = globallocalconverter_initialized(); + _ref_lat = math::degrees(_global_local_proj_ref.lat_rad); + _ref_lon = math::degrees(_global_local_proj_ref.lon_rad); + _ref_alt = _global_local_alt0; + + _is_global_cov_init = map_projection_initialized(&_global_local_proj_ref); if (!_map_ref.init_done && _is_global_cov_init) { // initialize global origin using the visual estimator reference @@ -58,7 +60,7 @@ void BlockLocalPositionEstimator::visionInit() if (!_altOriginInitialized) { _altOriginInitialized = true; _altOriginGlobal = true; - _altOrigin = globallocalconverter_initialized() ? _ref_alt : 0.0f; + _altOrigin = map_projection_initialized(&_global_local_proj_ref) ? _ref_alt : 0.0f; } } } diff --git a/src/modules/mavlink/mavlink_command_sender.cpp b/src/modules/mavlink/mavlink_command_sender.cpp index 85578b1335..c18675ed93 100644 --- a/src/modules/mavlink/mavlink_command_sender.cpp +++ b/src/modules/mavlink/mavlink_command_sender.cpp @@ -65,8 +65,13 @@ MavlinkCommandSender::~MavlinkCommandSender() px4_sem_destroy(&_lock); } -int MavlinkCommandSender::handle_vehicle_command(const struct vehicle_command_s &command, mavlink_channel_t channel) +int MavlinkCommandSender::handle_vehicle_command(const vehicle_command_s &command, mavlink_channel_t channel) { + // commands > uint16 are PX4 internal only + if (command.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) { + return 0; + } + lock(); CMD_DEBUG("new command: %d (channel: %d)", command.command, channel); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index b4e29d4fc1..a914d4a4a1 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2298,7 +2298,7 @@ Mavlink::task_main(int argc, char *argv[]) vehicle_command_ack_s command_ack; while ((get_free_tx_buf() >= COMMAND_ACK_TOTAL_LEN) && _vehicle_command_ack_sub.update(&command_ack)) { - if (!command_ack.from_external) { + if (!command_ack.from_external && command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) { mavlink_command_ack_t msg; msg.result = command_ack.result; msg.command = command_ack.command; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 904979ee30..2a9fb2b012 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -537,7 +537,7 @@ protected: _vehicle_command_sub.get_last_generation()); } - if (!cmd.from_external) { + if (!cmd.from_external && cmd.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) { PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component); MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel()); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5e21c0de82..01d201704a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -987,20 +987,20 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t vehicle_local_position_s local_pos{}; _vehicle_local_position_sub.copy(&local_pos); - if (!map_projection_initialized(&_global_local_proj_ref) - || (_global_local_proj_ref.timestamp != local_pos.ref_timestamp)) { - - map_projection_init_timestamped(&_global_local_proj_ref, local_pos.ref_lat, local_pos.ref_lon, local_pos.ref_timestamp); - _global_local_alt0 = local_pos.ref_alt; + if (!local_pos.xy_global || !local_pos.z_global) { + return; } + map_projection_reference_s global_local_proj_ref{}; + map_projection_init_timestamped(&global_local_proj_ref, local_pos.ref_lat, local_pos.ref_lon, local_pos.ref_timestamp); + // global -> local const double lat = target_global_int.lat_int / 1e7; const double lon = target_global_int.lon_int / 1e7; - map_projection_project(&_global_local_proj_ref, lat, lon, &setpoint.x, &setpoint.y); + map_projection_project(&global_local_proj_ref, lat, lon, &setpoint.x, &setpoint.y); if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_INT) { - setpoint.z = _global_local_alt0 - target_global_int.alt; + setpoint.z = local_pos.ref_alt - target_global_int.alt; } else if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { home_position_s home_position{}; @@ -1008,7 +1008,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t if (home_position.valid_alt) { const float alt = home_position.alt - target_global_int.alt; - setpoint.z = alt - _global_local_alt0; + setpoint.z = alt - local_pos.ref_alt; } else { // home altitude required @@ -1021,7 +1021,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t if (vehicle_global_position.terrain_alt_valid) { const float alt = target_global_int.alt + vehicle_global_position.terrain_alt; - setpoint.z = _global_local_alt0 - alt; + setpoint.z = local_pos.ref_alt - alt; } else { // valid terrain alt required @@ -1152,14 +1152,23 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m void MavlinkReceiver::handle_message_set_gps_global_origin(mavlink_message_t *msg) { - mavlink_set_gps_global_origin_t origin; - mavlink_msg_set_gps_global_origin_decode(msg, &origin); + mavlink_set_gps_global_origin_t gps_global_origin; + mavlink_msg_set_gps_global_origin_decode(msg, &gps_global_origin); - if (!globallocalconverter_initialized() && (origin.target_system == _mavlink->get_system_id())) { - /* Set reference point conversion of local coordiantes <--> global coordinates */ - globallocalconverter_init((double)origin.latitude * 1.0e-7, (double)origin.longitude * 1.0e-7, - (float)origin.altitude * 1.0e-3f, hrt_absolute_time()); - _global_ref_timestamp = hrt_absolute_time(); + if (gps_global_origin.target_system == _mavlink->get_system_id()) { + vehicle_command_s vcmd{}; + vcmd.param5 = (double)gps_global_origin.latitude * 1.e-7; + vcmd.param6 = (double)gps_global_origin.longitude * 1.e-7; + vcmd.param7 = (float)gps_global_origin.altitude * 1.e-3f; + vcmd.command = vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN; + vcmd.target_system = _mavlink->get_system_id(); + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = false; + vcmd.from_external = true; + vcmd.timestamp = hrt_absolute_time(); + _cmd_pub.publish(vcmd); } handle_request_message_command(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN); @@ -2484,34 +2493,32 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) /* local position */ { - double lat = hil_state.lat * 1e-7; - double lon = hil_state.lon * 1e-7; + const double lat = hil_state.lat * 1e-7; + const double lon = hil_state.lon * 1e-7; - if (!_hil_local_proj_inited) { - _hil_local_proj_inited = true; - _hil_local_alt0 = hil_state.alt / 1000.0f; + map_projection_reference_s global_local_proj_ref; + map_projection_init(&global_local_proj_ref, lat, lon); - map_projection_init(&_hil_local_proj_ref, lat, lon); - } + float global_local_alt0 = hil_state.alt / 1000.f; float x = 0.0f; float y = 0.0f; - map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); + map_projection_project(&global_local_proj_ref, lat, lon, &x, &y); vehicle_local_position_s hil_local_pos{}; hil_local_pos.timestamp = timestamp; - hil_local_pos.ref_timestamp = _hil_local_proj_ref.timestamp; - hil_local_pos.ref_lat = math::radians(_hil_local_proj_ref.lat_rad); - hil_local_pos.ref_lon = math::radians(_hil_local_proj_ref.lon_rad); - hil_local_pos.ref_alt = _hil_local_alt0; + hil_local_pos.ref_timestamp = global_local_proj_ref.timestamp; + hil_local_pos.ref_lat = math::degrees(global_local_proj_ref.lat_rad); + hil_local_pos.ref_lon = math::degrees(global_local_proj_ref.lon_rad); + hil_local_pos.ref_alt = global_local_alt0; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; hil_local_pos.v_xy_valid = true; hil_local_pos.v_z_valid = true; hil_local_pos.x = x; hil_local_pos.y = y; - hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f; + hil_local_pos.z = global_local_alt0 - hil_state.alt / 1000.0f; hil_local_pos.vx = hil_state.vx / 100.0f; hil_local_pos.vy = hil_state.vy / 100.0f; hil_local_pos.vz = hil_state.vz / 100.0f; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 10fcc389ce..6b21366441 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -395,10 +395,10 @@ private: uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status)}; // ORB publications (queue length > 1) - uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; - uORB::Publication _transponder_report_pub{ORB_ID(transponder_report)}; - uORB::Publication _cmd_ack_pub{ORB_ID(vehicle_command_ack)}; - uORB::Publication _cmd_pub{ORB_ID(vehicle_command)}; + uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; + uORB::Publication _transponder_report_pub{ORB_ID(transponder_report)}; + uORB::Publication _cmd_pub{ORB_ID(vehicle_command)}; + uORB::Publication _cmd_ack_pub{ORB_ID(vehicle_command_ack)}; // ORB subscriptions uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; @@ -428,15 +428,6 @@ private: uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {}; uint16_t _mom_switch_state{0}; - map_projection_reference_s _global_local_proj_ref{}; - float _global_local_alt0{NAN}; - - uint64_t _global_ref_timestamp{0}; - - map_projection_reference_s _hil_local_proj_ref{}; - float _hil_local_alt0{0.0f}; - bool _hil_local_proj_inited{false}; - hrt_abstime _last_utm_global_pos_com{0}; // Allocated if needed. diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index a5cdda49ff..d037aec79f 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -410,17 +410,24 @@ RoverPositionControl::Run() position_setpoint_triplet_poll(); - //Convert Local setpoints to global setpoints + // Convert Local setpoints to global setpoints if (_control_mode.flag_control_offboard_enabled) { - if (!globallocalconverter_initialized()) { - globallocalconverter_init(_local_pos.ref_lat, _local_pos.ref_lon, - _local_pos.ref_alt, _local_pos.ref_timestamp); + if (!map_projection_initialized(&_global_local_proj_ref) + || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) { - } else { - globallocalconverter_toglobal(_pos_sp_triplet.current.x, _pos_sp_triplet.current.y, _pos_sp_triplet.current.z, - &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon, &_pos_sp_triplet.current.alt); + map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon, + _local_pos.ref_timestamp); + _global_local_alt0 = _local_pos.ref_alt; } + + // local -> global + map_projection_reproject(&_global_local_proj_ref, + _pos_sp_triplet.current.x, _pos_sp_triplet.current.y, + &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon); + + _pos_sp_triplet.current.alt = _global_local_alt0 - _pos_sp_triplet.current.z; + _pos_sp_triplet.current.valid = true; } // update the reset counters in any case diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index e80c53f99d..bce7cf4d15 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -129,6 +129,9 @@ private: hrt_abstime _control_position_last_called{0}; /**::infinity(); hil_lpos.vz_max = std::numeric_limits::infinity(); hil_lpos.hagl_min = std::numeric_limits::infinity();