Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -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
{
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);
break;
case MSG_WHEEL_DISTANCE:
CHECK_PAYLOAD_SIZE(WHEEL_DISTANCE);
rover.send_wheel_encoder_distance(chan);
break;
case MSG_FENCE_STATUS:
CHECK_PAYLOAD_SIZE(FENCE_STATUS);
rover.send_fence_status(chan);
@ -518,6 +535,7 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
MSG_EKF_STATUS_REPORT,
MSG_VIBRATION,
MSG_RPM,
MSG_WHEEL_DISTANCE,
MSG_ESC_TELEMETRY,
};
static const ap_message STREAM_PARAMS_msgs[] = {

2
APMrover2/Rover.h
View File

@ -336,6 +336,7 @@ private:
// 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_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_ekf_update_ms; // system time of last encoder data push to EKF
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_pid_tuning(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);
// 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];
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
float delta_time;
const uint32_t latest_sensor_update_ms = g2.wheel_encoder.get_last_reading_ms(i);