RC_Channel: log auxillary function invocations

This commit is contained in:
Peter Barker 2021-04-28 19:13:53 +10:00 committed by Peter Barker
parent bd76d15df4
commit a12ed2c99f
2 changed files with 43 additions and 3 deletions

View File

@ -893,6 +893,32 @@ void RC_Channel::do_aux_function_mission_reset(const AuxSwitchPos ch_flag)
mission->reset();
}
bool RC_Channel::run_aux_function(aux_func_t ch_option, AuxSwitchPos pos, AuxFuncTriggerSource source)
{
const bool ret = do_aux_function(ch_option, pos);
// @LoggerMessage: AUXF
// @Description: Auixillary function invocation information
// @Field: TimeUS: Time since system startup
// @Field: function: ID of triggered function
// @Field: pos: switch position when function triggered
// @Field: source: source of auxillary function invocation
// @Field: result: true if function was successful
AP::logger().Write(
"AUXF",
"TimeUS,function,pos,source,result",
"s----",
"F----",
"QHBBB",
AP_HAL::micros64(),
uint16_t(ch_option),
uint8_t(pos),
uint8_t(source),
uint8_t(ret)
);
return ret;
}
bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag)
{
switch(ch_option) {

View File

@ -238,11 +238,20 @@ public:
HIGH // indicates auxiliary switch is in the high position (pwm >1800)
};
enum class AuxFuncTriggerSource : uint8_t {
INIT,
RC,
BUTTON,
MAVLINK,
MISSION,
};
bool read_3pos_switch(AuxSwitchPos &ret) const WARN_IF_UNUSED;
bool read_6pos_switch(int8_t& position) WARN_IF_UNUSED;
AuxSwitchPos get_aux_switch_pos() const;
virtual bool do_aux_function(aux_func_t ch_option, AuxSwitchPos);
// wrapper function around do_aux_function which allows us to log
bool run_aux_function(aux_func_t ch_option, AuxSwitchPos pos, AuxFuncTriggerSource source);
#if !HAL_MINIMIZE_FEATURES
const char *string_for_aux_function(AUX_FUNC function) const;
@ -266,6 +275,9 @@ protected:
virtual void init_aux_function(aux_func_t ch_option, AuxSwitchPos);
// virtual function to be overridden my subclasses
virtual bool do_aux_function(aux_func_t ch_option, AuxSwitchPos);
virtual void do_aux_function_armdisarm(const AuxSwitchPos ch_flag);
void do_aux_function_avoid_adsb(const AuxSwitchPos ch_flag);
void do_aux_function_avoid_proximity(const AuxSwitchPos ch_flag);
@ -483,8 +495,10 @@ public:
uint32_t last_input_ms() const { return last_update_ms; };
bool do_aux_function(RC_Channel::AUX_FUNC ch_option, RC_Channel::AuxSwitchPos pos) {
return rc_channel(0)->do_aux_function(ch_option, pos);
// method for other parts of the system (e.g. Button and mavlink)
// to trigger auxillary functions
bool run_aux_function(RC_Channel::AUX_FUNC ch_option, RC_Channel::AuxSwitchPos pos, RC_Channel::AuxFuncTriggerSource source) {
return rc_channel(0)->run_aux_function(ch_option, pos, source);
}
protected: