fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

it's working

Browse Source
This commit is contained in:
Jacob Dahl 2025-11-24 16:43:28 -09:00
parent 5c7e33d2cb
commit 55cdb4d192
5 changed files with 34 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/drivers/dshot/DShot.cpp
View File

@ -200,6 +200,8 @@ void DShot::select_next_command()
// Settings Request mask
uint8_t needs_settings_request_mask = _serial_telem_online_mask & ~_settings_requested_mask;
bool serial_telem_delay_elapsed = hrt_absolute_time() > _serial_telem_delay_until;
_current_command.clear();
if (_bdshot_telemetry_enabled && _bdshot_edt_enabled && needs_edt_request_mask) {
@ -220,7 +222,7 @@ void DShot::select_next_command()
_bdshot_edt_requested_mask |= (1 << next_motor_index);
PX4_INFO("ESC%d: requesting EDT at time %.2fs", next_motor_index + 1, (double)now / 1000000.);
} else if (_serial_telemetry_enabled && _bdshot_telemetry_enabled && needs_settings_request_mask) {
} else if (_serial_telemetry_enabled && _bdshot_telemetry_enabled && needs_settings_request_mask && serial_telem_delay_elapsed) {
// Settings Request next
int next_motor_index = 0;
@ -282,7 +284,8 @@ void DShot::select_next_command()
} else {
// All settings have been written
PX4_INFO("All settings written!");
PX4_INFO("All settings written at time %.2fs", (double)hrt_absolute_time() / 1000000.);
_dshot_programming_active = false;
// _programming_state = ProgrammingState::Save;
_current_command.command = DSHOT_CMD_SAVE_SETTINGS;
@ -290,7 +293,6 @@ void DShot::select_next_command()
_current_command.motor_mask = _am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index);
_programming_state = ProgrammingState::Idle;
// TODO: do we want to re-request and compare?
// Clear the written mask for this motor for next time
_settings_written_mask[0] = 0;
_settings_written_mask[1] = 0;
@ -299,7 +301,7 @@ void DShot::select_next_command()
// _serial_telem_online_mask &= ~(_am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index));
_settings_requested_mask &= ~(_am32_eeprom_write.index == 255 ? 255 : (1 << _am32_eeprom_write.index));
_serial_telem_delay_until = hrt_absolute_time() + 100_ms;
_serial_telem_delay_until = hrt_absolute_time() + 500_ms;
}
}
@ -444,7 +446,9 @@ bool DShot::process_serial_telemetry()
bool all_telem_sampled = false;
if (!_telemetry.commandResponseFinished()) {
_telemetry.parseCommandResponse();
if (_telemetry.commandResponseStarted()) {
_telemetry.parseCommandResponse();
}
} else {
@ -723,6 +727,10 @@ void DShot::handle_vehicle_commands()
handle_am32_request_eeprom(command);
break;
case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE:
PX4_INFO("VEHICLE_CMD_REQUEST_MESSAGE: param1: %f", (double)command.param1);
handle_am32_request_eeprom(command);
default:
break;
}
@ -803,16 +811,18 @@ void DShot::handle_configure_actuator(const vehicle_command_s &command)
void DShot::handle_am32_request_eeprom(const vehicle_command_s &command)
{
PX4_INFO("Received AM32_REQUEST_EEPROM");
PX4_INFO("index: %d", (int)command.param1);
PX4_INFO("esc_index: %d", (int)command.param2);
int index = command.param1;
int esc_index = command.param2;
// Mark as unread to re-trigger settings request
if (index == 255) {
if (esc_index == 255) {
PX4_INFO("mark all unread");
_settings_requested_mask = 0;
} else {
_settings_requested_mask &= ~(1 << index);
PX4_INFO("mark one unread");
_settings_requested_mask &= ~(1 << esc_index);
}
vehicle_command_ack_s command_ack = {};

7
src/drivers/dshot/DShotTelemetry.cpp
View File

@ -125,6 +125,8 @@ int DShotTelemetry::parseCommandResponse()
{
if (hrt_elapsed_time(&_command_response_start) > 1_s) {
PX4_WARN("Command response timed out: %d bytes received", _command_response_position);
PX4_WARN("At time %.2fs", (double)hrt_absolute_time() / 1000000.);
_command_response_motor_index = -1;
_command_response_start = 0;
_command_response_position = 0;
@ -275,6 +277,11 @@ bool DShotTelemetry::commandResponseFinished()
return _command_response_motor_index < 0;
}
bool DShotTelemetry::commandResponseStarted()
{
return _command_response_start > 0;
}
void DShotTelemetry::startTelemetryRequest()
{
_telemetry_request_start = hrt_absolute_time();

2
src/drivers/dshot/DShotTelemetry.h
View File

@ -55,6 +55,8 @@ public:
// Attempt to parse a command response. Returns the index of the ESC or -1 on failure.
int parseCommandResponse();
bool commandResponseFinished();
bool commandResponseStarted();
void setExpectCommandResponse(int motor_index, uint16_t command);
void initSettingsHandlers(ESCType esc_type, uint8_t output_mask);
void publish_esc_settings();

2
src/drivers/dshot/esc/AM32Settings.cpp
View File

@ -73,7 +73,7 @@ bool AM32Settings::decodeInfoResponse(const uint8_t *buf, int size)
return false;
}
// PX4_INFO("Successfully received AM32 settings from ESC%d", _esc_index + 1);
PX4_INFO("Successfully received AM32 settings from ESC%d", _esc_index + 1);
// Store data for retrieval later if requested
memcpy(&_eeprom_data, buf, EEPROM_SIZE);

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

@ -602,7 +602,11 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
uint16_t message_id = (uint16_t)roundf(vehicle_command.param1);
if (message_id == MAVLINK_MSG_ID_MESSAGE_INTERVAL) {
if (message_id == MAVLINK_MSG_ID_AM32_EEPROM) {
PX4_INFO("publishing MAV_CMD_REQUEST_MESSAGE for MAVLINK_MSG_ID_AM32_EEPROM");
_cmd_pub.publish(vehicle_command);
} else if (message_id == MAVLINK_MSG_ID_MESSAGE_INTERVAL) {
get_message_interval((int)(cmd_mavlink.param2 + 0.5f));
} else {