From 17b35b82095e400e89d1fe24d711e4053dd24936 Mon Sep 17 00:00:00 2001 From: ryanjAA Date: Sat, 29 Mar 2025 17:16:35 +0000 Subject: [PATCH 1/2] Update Mag quick Cal to accept heading arguments --- src/modules/commander/Commander.cpp | 27 ++++++++++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index cebc08ae9416..1c7a0658dda7 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -247,9 +247,30 @@ int Commander::custom_command(int argc, char *argv[]) } else if (!strcmp(argv[1], "mag")) { if (argc > 2 && (strcmp(argv[2], "quick") == 0)) { - // magnetometer quick calibration: VEHICLE_CMD_FIXED_MAG_CAL_YAW - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW); - + // Magnetometer quick calibration with optional heading + float heading_deg = 0.0f; + + if (argc > 3) { + char *end; + heading_deg = strtof(argv[3], &end); + + if (*end != '\0') { // Check if parsing failed + PX4_ERR("Invalid heading value: %s", argv[3]); + return 1; + } + + heading_deg = roundf(heading_deg); + } + + // Send command with heading (param1), other params zeroed (use GPS) + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW, + heading_deg, // param1: heading in degrees + 0.0f, // param2: unused + 0.0f, // param3: latitude (0 triggers GPS) + 0.0f, // param4: longitude (0 triggers GPS) + 0.0, // param5: unused + 0.0, // param6: unused + 0.0f); // param7: unused } else { // magnetometer calibration: param2 = 1 send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.0, 0.0, 0.f); From 2cec946d5a2ab6d3fba8001fae143879386f9814 Mon Sep 17 00:00:00 2001 From: ryanjAA Date: Mon, 31 Mar 2025 13:16:43 +0000 Subject: [PATCH 2/2] Fixed formatting --- src/modules/commander/Commander.cpp | 47 +++++++++++++++-------------- 1 file changed, 24 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 1c7a0658dda7..53f184a28349 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -248,29 +248,30 @@ int Commander::custom_command(int argc, char *argv[]) } else if (!strcmp(argv[1], "mag")) { if (argc > 2 && (strcmp(argv[2], "quick") == 0)) { // Magnetometer quick calibration with optional heading - float heading_deg = 0.0f; - - if (argc > 3) { - char *end; - heading_deg = strtof(argv[3], &end); - - if (*end != '\0') { // Check if parsing failed - PX4_ERR("Invalid heading value: %s", argv[3]); - return 1; - } - - heading_deg = roundf(heading_deg); - } - - // Send command with heading (param1), other params zeroed (use GPS) - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW, - heading_deg, // param1: heading in degrees - 0.0f, // param2: unused - 0.0f, // param3: latitude (0 triggers GPS) - 0.0f, // param4: longitude (0 triggers GPS) - 0.0, // param5: unused - 0.0, // param6: unused - 0.0f); // param7: unused + float heading_deg = 0.0f; + + if (argc > 3) { + char *end; + heading_deg = strtof(argv[3], &end); + + if (*end != '\0') { // Check if parsing failed + PX4_ERR("Invalid heading value: %s", argv[3]); + return 1; + } + + heading_deg = roundf(heading_deg); + } + + // Send command with heading (param1), other params zeroed (use GPS) + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW, + heading_deg, // param1: heading in degrees + 0.0f, // param2: unused + 0.0f, // param3: latitude (0 triggers GPS) + 0.0f, // param4: longitude (0 triggers GPS) + 0.0, // param5: unused + 0.0, // param6: unused + 0.0f); // param7: unused + } else { // magnetometer calibration: param2 = 1 send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.0, 0.0, 0.f);