fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 11:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2 support SET_GPS_GLOBAL_ORIGIN and remove globallocalconverter usage

Browse Source 
- 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
21 changed files with 212 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+37 -12
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();