From ebbbe0584a235ab664a4e25d5988f4706652d508 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 12 Jul 2017 11:02:51 +0900 Subject: [PATCH] Rover: integrate wheel encoder --- APMrover2/APMrover2.cpp | 2 ++ APMrover2/Log.cpp | 30 ++++++++++++++++++++++++++++++ APMrover2/Parameters.cpp | 4 ++++ APMrover2/Parameters.h | 3 +++ APMrover2/Rover.h | 3 +++ APMrover2/defines.h | 1 + APMrover2/sensors.cpp | 6 ++++++ APMrover2/system.cpp | 3 +++ 8 files changed, 52 insertions(+) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 3bcaefc6d4..256e243a67 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -54,6 +54,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK(update_alt, 10, 3400), SCHED_TASK(update_beacon, 50, 50), SCHED_TASK(update_visual_odom, 50, 50), + SCHED_TASK(update_wheel_encoder, 50, 50), SCHED_TASK(navigate, 10, 1600), SCHED_TASK(update_compass, 10, 2000), SCHED_TASK(update_commands, 10, 1000), @@ -283,6 +284,7 @@ void Rover::update_logging2(void) if (should_log(MASK_LOG_RC)) { Log_Write_RC(); + Log_Write_WheelEncoder(); } if (should_log(MASK_LOG_IMU)) { diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index 7c68179404..164278fe10 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -446,6 +446,33 @@ void Rover::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_targ DataFlash.WriteBlock(&pkt, sizeof(pkt)); } +// wheel encoder packet +struct PACKED log_WheelEncoder { + LOG_PACKET_HEADER; + uint64_t time_us; + float distance_0; + uint8_t quality_0; + float distance_1; + uint8_t quality_1; +}; + +// log wheel encoder information +void Rover::Log_Write_WheelEncoder() +{ + // return immediately if no wheel encoders are enabled + if (!g2.wheel_encoder.enabled(0) && !g2.wheel_encoder.enabled(1)) { + return; + } + struct log_WheelEncoder pkt = { + LOG_PACKET_HEADER_INIT(LOG_WHEELENCODER_MSG), + time_us : AP_HAL::micros64(), + distance_0 : g2.wheel_encoder.get_distance(0), + quality_0 : (uint8_t)constrain_float(g2.wheel_encoder.get_signal_quality(0),0.0f,100.0f), + distance_1 : g2.wheel_encoder.get_distance(1), + quality_1 : (uint8_t)constrain_float(g2.wheel_encoder.get_signal_quality(1),0.0f,100.0f) + }; + DataFlash.WriteBlock(&pkt, sizeof(pkt)); +} const LogStructure Rover::log_structure[] = { LOG_COMMON_STRUCTURES, @@ -467,6 +494,8 @@ const LogStructure Rover::log_structure[] = { "GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" }, { LOG_ERROR_MSG, sizeof(log_Error), "ERR", "QBB", "TimeUS,Subsys,ECode" }, + { LOG_WHEELENCODER_MSG, sizeof(log_WheelEncoder), + "WENC", "Qfbfb", "TimeUS,Dist0,Qual0,Dist1,Qual1" }, }; void Rover::log_init(void) @@ -532,5 +561,6 @@ void Rover::Log_Write_Baro(void) {} void Rover::Log_Arm_Disarm() {} void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {} void Rover::Log_Write_Steering() {} +void Rover::Log_Write_WheelEncoder() {} #endif // LOGGING_ENABLED diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 16e383af15..1583baea68 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -546,6 +546,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: MotorsUGV.cpp AP_SUBGROUPINFO(motors, "MOT_", 8, ParametersG2, AP_MotorsUGV), + // @Group: WENC_ + // @Path: ../libraries/AP_WheelEncoder/AP_WheelEncoder.cpp + AP_SUBGROUPINFO(wheel_encoder, "WENC_", 9, ParametersG2, AP_WheelEncoder), + AP_GROUPEND }; diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 0787166b9a..5308466e52 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -324,6 +324,9 @@ public: // Motor library AP_MotorsUGV motors; + + // wheel encoders + AP_WheelEncoder wheel_encoder; }; extern const AP_Param::Info var_info[]; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 78084acc19..c0a9f4dc63 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -76,6 +76,7 @@ #include // statistics library #include #include +#include // Configuration #include "config.h" @@ -469,6 +470,7 @@ private: void Log_Write_Baro(void); void Log_Write_Home_And_Origin(); void Log_Write_Vehicle_Startup_Messages(); + void Log_Write_WheelEncoder(); void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page); void log_init(void); void start_logging(); @@ -524,6 +526,7 @@ private: void update_beacon(); void init_visual_odom(); void update_visual_odom(); + void update_wheel_encoder(); void read_battery(void); void read_receiver_rssi(void); void read_sonars(void); diff --git a/APMrover2/defines.h b/APMrover2/defines.h index b98b8dac69..9643f09408 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -60,6 +60,7 @@ enum GuidedMode { #define LOG_ARM_DISARM_MSG 0x08 #define LOG_STEERING_MSG 0x0D #define LOG_GUIDEDTARGET_MSG 0x0E +#define LOG_WHEELENCODER_MSG 0x0F #define LOG_ERROR_MSG 0x13 #define TYPE_AIRSTART_MSG 0x00 diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index 39e8fd65a0..d6442bde81 100644 --- a/APMrover2/sensors.cpp +++ b/APMrover2/sensors.cpp @@ -92,6 +92,12 @@ void Rover::update_visual_odom() } } +// update wheel encoders +void Rover::update_wheel_encoder() +{ + g2.wheel_encoder.update(); +} + // read_battery - reads battery voltage and current and invokes failsafe // should be called at 10hz void Rover::read_battery(void) diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 5628000d0b..350763e49d 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -181,6 +181,9 @@ void Rover::init_ardupilot() g2.motors.init(); // init motors including setting servo out channels ranges init_rc_out(); // enable output + // init wheel encoders + g2.wheel_encoder.init(); + relay.init(); #if MOUNT == ENABLED