diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index ae83948aef536b..6849d22eb8bfc3 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -408,9 +408,9 @@ def config_option(self): Feature('Airspeed Drivers', 'SDP3X', 'AP_AIRSPEED_SDP3X_ENABLED', 'Enable SDP3X AIRSPEED', 0, 'AIRSPEED'), Feature('Airspeed Drivers', 'DRONECAN_ASPD', 'AP_AIRSPEED_DRONECAN_ENABLED', 'Enable DroneCAN AIRSPEED', 0, 'AIRSPEED,DroneCAN'), # NOQA: E501 - Feature('Actuators', 'Servo telem', 'AP_SERVO_TELEM_ENABLED', 'Enable servo telemetry library', 0, None), + Feature('Actuators', 'ServoTelem', 'AP_SERVO_TELEM_ENABLED', 'Enable servo telemetry library', 0, None), Feature('Actuators', 'Volz', 'AP_VOLZ_ENABLED', 'Enable Volz Protocol', 0, None), - Feature('Actuators', 'Volz_DroneCAN', 'AP_DRONECAN_VOLZ_FEEDBACK_ENABLED', 'Enable Volz DroneCAN Feedback', 0, "DroneCAN,Volz"), # noqa: E501 + Feature('Actuators', 'Volz_DroneCAN', 'AP_DRONECAN_VOLZ_FEEDBACK_ENABLED', 'Enable Volz DroneCAN Feedback', 0, "DroneCAN,Volz,ServoTelem"), # noqa: E501 Feature('Actuators', 'RobotisServo', 'AP_ROBOTISSERVO_ENABLED', 'Enable RobotisServo protocol', 0, None), Feature('Actuators', 'SBUS Output', 'AP_SBUSOUTPUT_ENABLED', 'Enable SBUS output on serial ports', 0, None), Feature('Actuators', 'FETTecOneWire', 'AP_FETTEC_ONEWIRE_ENABLED', 'Enable FETTec OneWire ESCs', 0, None), diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index c0707ebe79d8c5..6895a116bd1c2c 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -1448,7 +1448,7 @@ void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer, .voltage = msg.voltage * 0.2, .current = msg.current * 0.025, .duty_cycle = msg.motor_pwm * (100.0/255.0), - .motor_temperature_cdeg = (int16_t(msg.motor_temperature) - 50)) * 100, + .motor_temperature_cdeg = (int16_t(msg.motor_temperature) - 50) * 100, .valid_types = AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION | AP_Servo_Telem::TelemetryData::Types::VOLTAGE | AP_Servo_Telem::TelemetryData::Types::CURRENT | diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index 3e594bac079c23..14b71150218cac 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -350,7 +350,7 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { Canard::ObjCallback himark_servo_ServoInfo_cb{this, &AP_DroneCAN::handle_himark_servoinfo}; Canard::Subscriber himark_servo_ServoInfo_cb_listener{himark_servo_ServoInfo_cb, _driver_index}; #endif -#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED && AP_SERVO_TELEM_ENABLED +#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED Canard::ObjCallback volz_servo_ActuatorStatus_cb{this, &AP_DroneCAN::handle_actuator_status_Volz}; Canard::Subscriber volz_servo_ActuatorStatus_listener{volz_servo_ActuatorStatus_cb, _driver_index}; #endif @@ -418,7 +418,7 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { #if AP_SERVO_TELEM_ENABLED void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg); #endif -#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED && AP_SERVO_TELEM_ENABLED +#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg); #endif void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg);