dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
This commit is contained in:
Daniel Agar 2021-03-05 18:25:14 -05:00 committed by GitHub
parent 12a4b0334f
commit 263b00b65f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
21 changed files with 212 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
msg/vehicle_command.msg
View File

@ -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

2
msg/vehicle_command_ack.msg
View File

@ -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

49
src/modules/commander/Commander.cpp
View File

@ -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<double>(NAN),
const double param6 = static_cast<double>(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();

24
src/modules/ekf2/EKF2.cpp
View File

@ -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<double>(altitude));
}
}
}
}
bool imu_updated = false;
imuSample imu_sample_new {};

2
src/modules/ekf2/EKF2.hpp
View File

@ -77,6 +77,7 @@
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_imu.h>
@ -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)};

11
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
View File

@ -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;
}
}
}
}

9
src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
View File

@ -53,6 +53,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/home_position.h>
#include <lib/ecl/geo/geo.h>
#include <lib/weather_vane/WeatherVane.hpp>
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;

29
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
View File

@ -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
}

18
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@ -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<double>(altitude));
}
}
}
sensor_combined_s imu;
if (!_sensors_sub.update(&imu)) {

7
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

@ -12,6 +12,7 @@
// uORB Subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_angular_velocity.h>
@ -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<actuator_armed_s> _sub_armed{ORB_ID(actuator_armed)};
uORB::SubscriptionData<vehicle_land_detected_s> _sub_land{ORB_ID(vehicle_land_detected)};
uORB::SubscriptionData<vehicle_attitude_s> _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;

10
src/modules/local_position_estimator/sensors/mocap.cpp
View File

@ -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;
}
}
}

10
src/modules/local_position_estimator/sensors/vision.cpp
View File

@ -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;
}
}
}

7
src/modules/mavlink/mavlink_command_sender.cpp
View File

@ -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);

2
src/modules/mavlink/mavlink_main.cpp
View File

@ -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;

2
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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());

65
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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;

17
src/modules/mavlink/mavlink_receiver.h
View File

@ -395,10 +395,10 @@ private:
uORB::PublicationMulti<radio_status_s> _radio_status_pub{ORB_ID(radio_status)};
// ORB publications (queue length > 1)
uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
uORB::Publication<transponder_report_s> _transponder_report_pub{ORB_ID(transponder_report)};
uORB::Publication<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_command_s> _cmd_pub{ORB_ID(vehicle_command)};
uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
uORB::Publication<transponder_report_s> _transponder_report_pub{ORB_ID(transponder_report)};
uORB::Publication<vehicle_command_s> _cmd_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_command_ack_s> _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.

21
src/modules/rover_pos_control/RoverPositionControl.cpp
View File

@ -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

3
src/modules/rover_pos_control/RoverPositionControl.hpp
View File

@ -129,6 +129,9 @@ private:
hrt_abstime _control_position_last_called{0}; /**<last call of control_position */
hrt_abstime _manual_setpoint_last_called{0};
map_projection_reference_s _global_local_proj_ref{};
float _global_local_alt0{NAN};
/* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on
the throttle. For now just assuming a proportional scaler between controlled airspeed and throttle output.*/
PID_t _speed_ctrl{};

10
src/modules/simulator/simulator.h
View File

@ -261,14 +261,8 @@ private:
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
// hil map_ref data
struct map_projection_reference_s _hil_local_proj_ref {};
bool _hil_local_proj_inited{false};
double _hil_ref_lat{0};
double _hil_ref_lon{0};
float _hil_ref_alt{0.0f};
uint64_t _hil_ref_timestamp{0};
map_projection_reference_s _global_local_proj_ref{};
float _global_local_alt0{NAN};
vehicle_status_s _vehicle_status{};

24
src/modules/simulator/simulator_mavlink.cpp
View File

@ -504,25 +504,21 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
}
/* local position */
struct vehicle_local_position_s hil_lpos = {};
vehicle_local_position_s hil_lpos{};
{
hil_lpos.timestamp = timestamp;
double lat = hil_state.lat * 1e-7;
double lon = hil_state.lon * 1e-7;
if (!_hil_local_proj_inited) {
_hil_local_proj_inited = true;
map_projection_init(&_hil_local_proj_ref, lat, lon);
_hil_ref_timestamp = timestamp;
_hil_ref_lat = lat;
_hil_ref_lon = lon;
_hil_ref_alt = hil_state.alt / 1000.0f;
if (!map_projection_initialized(&_global_local_proj_ref)) {
map_projection_init(&_global_local_proj_ref, lat, lon);
_global_local_alt0 = hil_state.alt / 1000.f;
}
float x;
float y;
map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
map_projection_project(&_global_local_proj_ref, lat, lon, &x, &y);
hil_lpos.timestamp = timestamp;
hil_lpos.xy_valid = true;
hil_lpos.z_valid = true;
@ -530,7 +526,7 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
hil_lpos.v_z_valid = true;
hil_lpos.x = x;
hil_lpos.y = y;
hil_lpos.z = _hil_ref_alt - hil_state.alt / 1000.0f;
hil_lpos.z = _global_local_alt0 - hil_state.alt / 1000.0f;
hil_lpos.vx = hil_state.vx / 100.0f;
hil_lpos.vy = hil_state.vy / 100.0f;
hil_lpos.vz = hil_state.vz / 100.0f;
@ -538,10 +534,10 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
hil_lpos.heading = euler.psi();
hil_lpos.xy_global = true;
hil_lpos.z_global = true;
hil_lpos.ref_lat = _hil_ref_lat;
hil_lpos.ref_lon = _hil_ref_lon;
hil_lpos.ref_alt = _hil_ref_alt;
hil_lpos.ref_timestamp = _hil_ref_timestamp;
hil_lpos.ref_timestamp = _global_local_proj_ref.timestamp;
hil_lpos.ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
hil_lpos.ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
hil_lpos.ref_alt = _global_local_alt0;
hil_lpos.vxy_max = std::numeric_limits<float>::infinity();
hil_lpos.vz_max = std::numeric_limits<float>::infinity();
hil_lpos.hagl_min = std::numeric_limits<float>::infinity();