Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: send mavlink WHEEL_DISTANCE message

Browse Source
This commit is contained in:
Pavlo Kolomiiets 2019-01-26 11:14:00 +09:00 committed by Randy Mackay
parent 195b053e1a
commit 3d160b7832
3 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
APMrover2/GCS_Mavlink.cpp
View File

@ -287,6 +287,18 @@ void Rover::send_wheel_encoder(mavlink_channel_t chan)
} }
} }
void Rover::send_wheel_encoder_distance(mavlink_channel_t chan)
{
// send wheel encoder data using wheel_distance message
if (g2.wheel_encoder.num_sensors() > 0) {
double distances[16] {};
for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) {
distances[i] = wheel_encoder_last_distance_m[i];
}
mavlink_msg_wheel_distance_send(chan, 1000UL * wheel_encoder_last_ekf_update_ms, g2.wheel_encoder.num_sensors(), distances);
}
}
uint8_t GCS_MAVLINK_Rover::sysid_my_gcs() const uint8_t GCS_MAVLINK_Rover::sysid_my_gcs() const
{ {
return rover.g.sysid_my_gcs; return rover.g.sysid_my_gcs;
@ -346,6 +358,11 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
rover.send_wheel_encoder(chan); rover.send_wheel_encoder(chan);
break; break;
case MSG_WHEEL_DISTANCE:
CHECK_PAYLOAD_SIZE(WHEEL_DISTANCE);
rover.send_wheel_encoder_distance(chan);
break;
case MSG_FENCE_STATUS: case MSG_FENCE_STATUS:
CHECK_PAYLOAD_SIZE(FENCE_STATUS); CHECK_PAYLOAD_SIZE(FENCE_STATUS);
rover.send_fence_status(chan); rover.send_fence_status(chan);
@ -518,6 +535,7 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
MSG_EKF_STATUS_REPORT, MSG_EKF_STATUS_REPORT,
MSG_VIBRATION, MSG_VIBRATION,
MSG_RPM, MSG_RPM,
MSG_WHEEL_DISTANCE,
MSG_ESC_TELEMETRY, MSG_ESC_TELEMETRY,
}; };
static const ap_message STREAM_PARAMS_msgs[] = { static const ap_message STREAM_PARAMS_msgs[] = {

2
APMrover2/Rover.h
View File

@ -336,6 +336,7 @@ private:
// last wheel encoder update times // last wheel encoder update times
float wheel_encoder_last_angle_rad[WHEELENCODER_MAX_INSTANCES]; // distance in radians at time of last update to EKF float wheel_encoder_last_angle_rad[WHEELENCODER_MAX_INSTANCES]; // distance in radians at time of last update to EKF
float wheel_encoder_last_distance_m[WHEELENCODER_MAX_INSTANCES]; // distance in meters at time of last update to EKF (for reporting to GCS)
uint32_t wheel_encoder_last_update_ms[WHEELENCODER_MAX_INSTANCES]; // system time of last ping from each encoder uint32_t wheel_encoder_last_update_ms[WHEELENCODER_MAX_INSTANCES]; // system time of last ping from each encoder
uint32_t wheel_encoder_last_ekf_update_ms; // system time of last encoder data push to EKF uint32_t wheel_encoder_last_ekf_update_ms; // system time of last encoder data push to EKF
float wheel_encoder_rpm[WHEELENCODER_MAX_INSTANCES]; // for reporting to GCS float wheel_encoder_rpm[WHEELENCODER_MAX_INSTANCES]; // for reporting to GCS
@ -443,6 +444,7 @@ private:
void send_servo_out(mavlink_channel_t chan); void send_servo_out(mavlink_channel_t chan);
void send_pid_tuning(mavlink_channel_t chan); void send_pid_tuning(mavlink_channel_t chan);
void send_wheel_encoder(mavlink_channel_t chan); void send_wheel_encoder(mavlink_channel_t chan);
void send_wheel_encoder_distance(mavlink_channel_t chan);
void send_fence_status(mavlink_channel_t chan); void send_fence_status(mavlink_channel_t chan);
// Log.cpp // Log.cpp

3
APMrover2/sensors.cpp
View File

@ -99,6 +99,9 @@ void Rover::update_wheel_encoder()
const float delta_angle = curr_angle_rad - wheel_encoder_last_angle_rad[i]; const float delta_angle = curr_angle_rad - wheel_encoder_last_angle_rad[i];
wheel_encoder_last_angle_rad[i] = curr_angle_rad; wheel_encoder_last_angle_rad[i] = curr_angle_rad;
// save cumulative distances at current time (in meters)
wheel_encoder_last_distance_m[i] = g2.wheel_encoder.get_distance(i);
// calculate delta time // calculate delta time
float delta_time; float delta_time;
const uint32_t latest_sensor_update_ms = g2.wheel_encoder.get_last_reading_ms(i); const uint32_t latest_sensor_update_ms = g2.wheel_encoder.get_last_reading_ms(i);