diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp index 4dac4792b3..c5318ef3ce 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp @@ -14,7 +14,7 @@ #include #include -#include +#include // Extended Position Data Format defines -100 as 0x0080 decimal 128, we map this to a PWM of 1000 (if range is default) #define PWM_POSITION_MIN 1000 @@ -77,7 +77,7 @@ void AP_Volz_Protocol::init(void) } } -#if HAL_LOGGING_ENABLED +#if AP_SERVO_TELEM_ENABLED // Request telem data, cycling through each servo and telem item void AP_Volz_Protocol::request_telem() { @@ -144,7 +144,7 @@ void AP_Volz_Protocol::loop() hal.scheduler->delay_microseconds(100); } -#if HAL_LOGGING_ENABLED +#if AP_SERVO_TELEM_ENABLED // Send a command for each servo, then one telem request const uint8_t num_servos = __builtin_popcount(bitmask.get()); if (sent_count < num_servos) { @@ -162,7 +162,7 @@ void AP_Volz_Protocol::loop() read_telem(); } -#else // No logging, send only +#else // No telem, send only send_position_cmd(); #endif } @@ -208,7 +208,7 @@ void AP_Volz_Protocol::send_position_cmd() send_command(cmd); -#if HAL_LOGGING_ENABLED +#if AP_SERVO_TELEM_ENABLED { // Update the commanded angle WITH_SEMAPHORE(telem.sem); @@ -221,7 +221,7 @@ void AP_Volz_Protocol::send_position_cmd() } } -#if HAL_LOGGING_ENABLED +#if AP_SERVO_TELEM_ENABLED void AP_Volz_Protocol::process_response(const CMD &cmd) { // Convert to 0 indexed @@ -348,7 +348,7 @@ void AP_Volz_Protocol::read_telem() // Used up all attempts without running out of data. // Really should not end up here } -#endif // HAL_LOGGING_ENABLED +#endif // AP_SERVO_TELEM_ENABLED // Called each time the servo outputs are sent void AP_Volz_Protocol::update() @@ -376,11 +376,11 @@ void AP_Volz_Protocol::update() servo_pwm[i] = (pwm == 0) ? c->get_trim() : pwm; } -#if HAL_LOGGING_ENABLED - // take semaphore and log all channels at 5 Hz - const uint32_t now_ms = AP_HAL::millis(); - if ((now_ms - telem.last_log_ms) > 200) { - telem.last_log_ms = now_ms; +#if AP_SERVO_TELEM_ENABLED + // Report telem data + AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton(); + if (servo_telem != nullptr) { + const uint32_t now_ms = AP_HAL::millis(); WITH_SEMAPHORE(telem.sem); for (uint8_t i=0; iupdate_telem_data(i, telem_data); } } -#endif // HAL_LOGGING_ENABLED +#endif // AP_SERVO_TELEM_ENABLED } // Return the crc for a given command packet diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h index f14df30230..d17fa76570 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h @@ -44,8 +44,7 @@ #include #include #include -#include - +#include class AP_Volz_Protocol { public: @@ -110,7 +109,7 @@ private: AP_Int16 range; bool initialised; -#if HAL_LOGGING_ENABLED +#if AP_SERVO_TELEM_ENABLED // Request telem data, cycling through each servo and telem item void request_telem(); @@ -143,7 +142,6 @@ private: int16_t motor_temp_deg; int16_t pcb_temp_deg; } data[NUM_SERVO_CHANNELS]; - uint32_t last_log_ms; } telem; #endif