aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_PiccoloCAN: Send servo commands for active channels

Browse Source 
- For each channel, check if a non-motor function is assigned
- If so, send the function value out to the servo
This commit is contained in:
Oliver Walters 2020-11-11 16:05:04 +11:00 committed by Andrew Tridgell
parent 7b0da02a18
commit 115c1224de
1 changed files with 25 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp
View File

@ -292,23 +292,31 @@ void AP_PiccoloCAN::update()
for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) {
if (is_servo_channel_active(ii)) {
// TODO - Extract output command here
uint16_t output = 0;
SRV_Channel::Aux_servo_function_t function = SRV_Channels::channel_function(ii);
if (SRV_Channels::get_output_pwm(function, output)) {
_servo_info[ii].command = output;
_servo_info[ii].newCommand = true;
}
}
}
/* Read out the ESC commands from the channel mixer */
for (uint8_t i = 0; i < PICCOLO_CAN_MAX_NUM_ESC; i++) {
for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_ESC; ii++) {
if (is_esc_channel_active(i)) {
if (is_esc_channel_active(ii)) {
uint16_t output = 0;
SRV_Channel::Aux_servo_function_t motor_function = SRV_Channels::get_motor_function(i);
SRV_Channel::Aux_servo_function_t motor_function = SRV_Channels::get_motor_function(ii);
if (SRV_Channels::get_output_pwm(motor_function, output)) {
_esc_info[i].command = output;
_esc_info[i].newCommand = true;
_esc_info[ii].command = output;
_esc_info[ii].newCommand = true;
}
}
}
@ -731,14 +739,20 @@ bool AP_PiccoloCAN::is_servo_channel_active(uint8_t chan)
return false;
}
// Check if a servo function is assigned for this motor channel
// TODO - ?
if (1) {
return true;
SRV_Channel::Aux_servo_function_t function = SRV_Channels::channel_function(chan);
// Ignore if the servo channel does not have a function assigned
if (function == SRV_Channel::k_none) {
return false;
}
return false;
// Ignore if the assigned function is a motor function
if (SRV_Channel::is_motor(function)) {
return false;
}
// We can safely say that the particular servo channel is active
return true;
}
/**