mirror of https://github.com/ArduPilot/ardupilot
AP_BoardConfig: allow programmatic setting of default safety mask
This commit is contained in:
parent
750aa2ef8a
commit
7bf81c44b0
|
@ -168,3 +168,12 @@ void AP_BoardConfig::init()
|
||||||
hal.util->set_imu_target_temp((int8_t *)&_imu_target_temperature);
|
hal.util->set_imu_target_temp((int8_t *)&_imu_target_temperature);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// set default value for BRD_SAFETY_MASK
|
||||||
|
void AP_BoardConfig::set_default_safety_ignore_mask(uint16_t mask)
|
||||||
|
{
|
||||||
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||||
|
px4.ignore_safety_channels.set_default(mask);
|
||||||
|
px4_setup_safety_mask();
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
|
@ -59,6 +59,9 @@ public:
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
// set default value for BRD_SAFETY_MASK
|
||||||
|
void set_default_safety_ignore_mask(uint16_t mask);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
AP_Int16 vehicleSerialNumber;
|
AP_Int16 vehicleSerialNumber;
|
||||||
|
|
||||||
|
@ -81,6 +84,7 @@ private:
|
||||||
void px4_setup(void);
|
void px4_setup(void);
|
||||||
void px4_setup_pwm(void);
|
void px4_setup_pwm(void);
|
||||||
void px4_setup_safety(void);
|
void px4_setup_safety(void);
|
||||||
|
void px4_setup_safety_mask(void);
|
||||||
void px4_setup_uart(void);
|
void px4_setup_uart(void);
|
||||||
void px4_setup_sbus(void);
|
void px4_setup_sbus(void);
|
||||||
void px4_setup_canbus(void);
|
void px4_setup_canbus(void);
|
||||||
|
|
|
@ -132,7 +132,7 @@ void AP_BoardConfig::px4_setup_uart()
|
||||||
/*
|
/*
|
||||||
setup safety switch
|
setup safety switch
|
||||||
*/
|
*/
|
||||||
void AP_BoardConfig::px4_setup_safety()
|
void AP_BoardConfig::px4_setup_safety_mask()
|
||||||
{
|
{
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||||
// setup channels to ignore the armed state
|
// setup channels to ignore the armed state
|
||||||
|
@ -157,6 +157,14 @@ void AP_BoardConfig::px4_setup_safety()
|
||||||
close(px4io_fd);
|
close(px4io_fd);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
setup safety switch
|
||||||
|
*/
|
||||||
|
void AP_BoardConfig::px4_setup_safety()
|
||||||
|
{
|
||||||
|
px4_setup_safety_mask();
|
||||||
|
|
||||||
if (px4.safety_enable.get() == 0) {
|
if (px4.safety_enable.get() == 0) {
|
||||||
hal.rcout->force_safety_off();
|
hal.rcout->force_safety_off();
|
||||||
|
|
Loading…
Reference in New Issue