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

SRV_Channel: added get_motor_num()

Browse Source 
map back from a channel to a motor number, for ESC telemetry reporting
This commit is contained in:
Andrew Tridgell 2021-12-09 10:50:32 +11:00 committed by Tom Pittenger
parent 604f2430f8
commit 5e43370e35
2 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/SRV_Channel/SRV_Channel.cpp
View File

@ -277,3 +277,19 @@ bool SRV_Channel::is_control_surface(SRV_Channel::Aux_servo_function_t function)
return false;
}
// return the motor number of a channel, or -1 if not a motor
// return 0 for first motor
int8_t SRV_Channel::get_motor_num(void) const
{
const auto k_function = get_function();
switch (k_function) {
case k_motor1 ... k_motor8:
return int8_t(uint16_t(k_function) - k_motor1);
case k_motor9 ... k_motor12:
return 8 + int8_t(uint16_t(k_function) - k_motor9);
default:
break;
}
return -1;
}

3
libraries/SRV_Channel/SRV_Channel.h
View File

@ -240,6 +240,9 @@ public:
return (SRV_Channel::Aux_servo_function_t)function.get();
}
// return the motor number of a channel, or -1 if not a motor
int8_t get_motor_num(void) const;
// set and save function for channel. Used in upgrade of parameters in plane
void function_set_and_save(SRV_Channel::Aux_servo_function_t f) {
function.set_and_save(int8_t(f));