mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
HAL_PX4: fixed safety switch light pattern
this was broken by the recent CAN merge. My fault for not checking it carefully. We have to publish the armed state so the fmu driver on pixracer can update the light
This commit is contained in:
parent
596b20eb07
commit
ca4cde9770
@ -418,6 +418,12 @@ void PX4RCOutput::_arm_actuators(bool arm)
|
||||
}
|
||||
_armed.lockdown = false;
|
||||
_armed.force_failsafe = false;
|
||||
|
||||
if (_actuator_armed_pub == nullptr) {
|
||||
_actuator_armed_pub = orb_advertise(ORB_ID(actuator_armed), &_armed);
|
||||
} else {
|
||||
orb_publish(ORB_ID(actuator_armed), _actuator_armed_pub, &_armed);
|
||||
}
|
||||
}
|
||||
|
||||
void PX4RCOutput::_send_outputs(void)
|
||||
@ -464,6 +470,8 @@ void PX4RCOutput::_send_outputs(void)
|
||||
}
|
||||
}
|
||||
if (to_send > 0) {
|
||||
_arm_actuators(true);
|
||||
|
||||
::write(_pwm_fd, _period, to_send*sizeof(_period[0]));
|
||||
}
|
||||
if (_max_channel > _servo_count) {
|
||||
|
@ -3,6 +3,7 @@
|
||||
#include "AP_HAL_PX4.h"
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
|
||||
#define PX4_NUM_OUTPUT_CHANNELS 16
|
||||
|
||||
@ -61,9 +62,10 @@ private:
|
||||
} _outputs[ORB_MULTI_MAX_INSTANCES] {};
|
||||
actuator_armed_s _armed;
|
||||
|
||||
orb_advert_t _actuator_direct_pub = nullptr;
|
||||
uint16_t _esc_pwm_min = 0;
|
||||
uint16_t _esc_pwm_max = 0;
|
||||
orb_advert_t _actuator_direct_pub;
|
||||
orb_advert_t _actuator_armed_pub;
|
||||
uint16_t _esc_pwm_min;
|
||||
uint16_t _esc_pwm_max;
|
||||
|
||||
void _init_alt_channels(void);
|
||||
void _publish_actuators(void);
|
||||
|
Loading…
Reference in New Issue
Block a user