Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_DroneCAN: make himark servo support optional

This commit is contained in:
Andrew Tridgell 2023-09-02 16:37:27 +10:00 committed by Peter Barker
parent d70027f83a
commit b33cf11bfb
2 changed files with 24 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_DroneCAN/AP_DroneCAN.cpp
View File

@ -284,9 +284,11 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters)
esc_hobbywing_raw.set_timeout_ms(2);
esc_hobbywing_raw.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
#endif
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
himark_out.set_timeout_ms(2);
himark_out.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
#endif
rgb_led.set_timeout_ms(20);
rgb_led.set_priority(CANARD_TRANSFER_PRIORITY_LOW);
@ -406,9 +408,12 @@ void AP_DroneCAN::loop(void)
const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get());
if (now - _SRV_last_send_us >= servo_period_us) {
_SRV_last_send_us = now;
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
if (option_is_set(Options::USE_HIMARK_SERVO)) {
SRV_send_himark();
} else {
} else
#endif
{
SRV_send_actuator();
}
for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) {
@ -622,6 +627,7 @@ void AP_DroneCAN::SRV_send_actuator(void)
} while (repeat_send);
}
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
/*
Himark servo output. This uses com.himark.servo.ServoCmd packets
*/
@ -653,6 +659,7 @@ void AP_DroneCAN::SRV_send_himark(void)
himark_out.broadcast(msg);
}
#endif // AP_DRONECAN_HIMARK_SERVO_SUPPORT
void AP_DroneCAN::SRV_send_esc(void)
{
@ -1192,6 +1199,7 @@ void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const
0, 0, 0, 0, 0, 0);
}
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
/*
handle himark ServoInfo message
*/
@ -1211,6 +1219,7 @@ void AP_DroneCAN::handle_himark_servoinfo(const CanardRxTransfer& transfer, cons
msg.pcb_temp*0.2-40,
msg.error_status);
}
#endif // AP_DRONECAN_HIMARK_SERVO_SUPPORT
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg)

14
libraries/AP_DroneCAN/AP_DroneCAN.h
View File

@ -54,6 +54,10 @@
#define AP_DRONECAN_HOBBYWING_ESC_SUPPORT (BOARD_FLASH_SIZE>1024)
#endif
#ifndef AP_DRONECAN_HIMARK_SERVO_SUPPORT
#define AP_DRONECAN_HIMARK_SERVO_SUPPORT (BOARD_FLASH_SIZE>1024)
#endif
// fwd-declare callback classes
class AP_DroneCAN_DNA_Server;
@ -145,7 +149,9 @@ private:
///// SRV output /////
void SRV_send_actuator();
void SRV_send_esc();
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
void SRV_send_himark();
#endif
//scale servo output appropriately before sending
int16_t scale_esc_output(uint8_t idx);
@ -253,7 +259,10 @@ private:
Canard::Publisher<ardupilot_indication_SafetyState> safety_state{canard_iface};
Canard::Publisher<uavcan_equipment_safety_ArmingStatus> arming_status{canard_iface};
Canard::Publisher<ardupilot_indication_NotifyState> notify_state{canard_iface};
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
Canard::Publisher<com_himark_servo_ServoCmd> himark_out{canard_iface};
#endif
#if AP_DRONECAN_SEND_GPS
Canard::Publisher<uavcan_equipment_gnss_Fix2> gnss_fix2{canard_iface};
@ -324,6 +333,10 @@ private:
void handle_hobbywing_StatusMsg1(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg1& msg);
void handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg2& msg);
#endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
void handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg);
#endif
// incoming button handling
void handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg);
@ -331,7 +344,6 @@ private:
void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg);
void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg);
void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg);
void handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg);
static bool is_esc_data_index_valid(const uint8_t index);
void handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg);
void handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp);