Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-13 10:03:57 -03:00
Code Issues Packages Projects Releases Wiki Activity

Rover: reporting wheel encoder rpm

Browse Source
This commit is contained in:
Randy Mackay 2017-07-21 11:56:39 +09:00
parent ae487aa99b
commit ddc70058d5
3 changed files with 24 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
APMrover2/GCS_Mavlink.cpp
View File

@ -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);
}
}

2
APMrover2/Rover.h
View File

@ -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);

8
APMrover2/sensors.cpp
View File

@ -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