diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 29fe8ef53876eb41d13fac0fcf97e4e861964340..d0dd65fbba82e10137453f528c8a5f475936681c 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -269,6 +269,14 @@ void Rover::send_current_waypoint(mavlink_channel_t chan) mavlink_msg_mission_current_send(chan, mission.get_current_nav_index()); } +void Rover::send_wheel_encoder(mavlink_channel_t chan) +{ + // send wheel encoder data using rpm message + if (g2.wheel_encoder.enabled(0) || g2.wheel_encoder.enabled(1)) { + mavlink_msg_rpm_send(chan, wheel_encoder_rpm[0], wheel_encoder_rpm[1]); + } +} + uint8_t GCS_MAVLINK_Rover::sysid_my_gcs() const { return rover.g.sysid_my_gcs; @@ -415,6 +423,11 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id) send_distance_sensor(rover.rangefinder); break; + case MSG_RPM: + CHECK_PAYLOAD_SIZE(RPM); + rover.send_wheel_encoder(chan); + break; + case MSG_MOUNT_STATUS: #if MOUNT == ENABLED CHECK_PAYLOAD_SIZE(MOUNT_STATUS); @@ -481,7 +494,6 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id) case MSG_TERRAIN: case MSG_OPTICAL_FLOW: case MSG_GIMBAL_REPORT: - case MSG_RPM: case MSG_POSITION_TARGET_GLOBAL_INT: case MSG_LANDING: break; // just here to prevent a warning @@ -691,6 +703,7 @@ GCS_MAVLINK_Rover::data_stream_send(void) send_message(MSG_MOUNT_STATUS); send_message(MSG_EKF_STATUS_REPORT); send_message(MSG_VIBRATION); + send_message(MSG_RPM); } } diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index cdd35b294d02cc0c713460eb1f913960d0306dca..1e322af1e57bca5003bae46e79fe5cfc6bf3f437 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -415,6 +415,7 @@ private: float wheel_encoder_last_angle_rad[WHEELENCODER_MAX_INSTANCES]; // distance in radians at time of last update to EKF 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 // True when we are doing motor test bool motor_test; @@ -460,6 +461,7 @@ private: void send_pid_tuning(mavlink_channel_t chan); void send_rangefinder(mavlink_channel_t chan); void send_current_waypoint(mavlink_channel_t chan); + void send_wheel_encoder(mavlink_channel_t chan); void gcs_data_stream_send(void); void gcs_update(void); void gcs_retry_deferred(void); diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index e720d9d400e49ab38375149b11525ee2d8571bba..a8b2cd6bdbe76d102ec6a51d17e91e34fe188e5e 100644 --- a/APMrover2/sensors.cpp +++ b/APMrover2/sensors.cpp @@ -148,6 +148,14 @@ void Rover::update_wheel_encoder() * posOffset is the XYZ body frame position of the wheel hub (m) */ EKF3.writeWheelOdom(delta_angle, delta_time, wheel_encoder_last_update_ms[i], g2.wheel_encoder.get_position(i), g2.wheel_encoder.get_wheel_radius(i)); + + // calculate rpm for reporting to GCS + if (is_positive(delta_time)) { + wheel_encoder_rpm[i] = (delta_angle / M_2PI) / (delta_time / 60.0f); + } else { + wheel_encoder_rpm[i] = 0.0f; + } + } // record system time update for next iteration