void
CameraTrigger::control(bool on)
{
// always execute, even if already on
// to reset timings if necessary
if (on) {
// schedule trigger on and off calls
hrt_call_every(&_engagecall, 0, (_interval * 1000),
(hrt_callout)&CameraTrigger::engage, this);
// schedule trigger on and off calls
hrt_call_every(&_disengagecall, 0 + (_activation_time * 1000), (_interval * 1000),
(hrt_callout)&CameraTrigger::disengage, this);
} else {
// cancel all calls
hrt_cancel(&_engagecall);
hrt_cancel(&_disengagecall);
// ensure that the pin is off
hrt_call_after(&_disengagecall, 0,
(hrt_callout)&CameraTrigger::disengage, this);
}
_trigger_enabled = on;
}
void
CameraTrigger::stop()
{
hrt_cancel(&_firecall);
hrt_cancel(&_pollcall);
if (camera_trigger::g_camera_trigger != nullptr) {
delete (camera_trigger::g_camera_trigger);
}
}
void
FXOS8700CQ::stop()
{
hrt_cancel(&_accel_call);
hrt_cancel(&_mag_call);
/* reset internal states */
memset(_last_accel, 0, sizeof(_last_accel));
/* discard unread data in the buffers */
_accel_reports->flush();
_mag_reports->flush();
}
void
CameraTrigger::enable_keep_alive(bool on)
{
if (on) {
// schedule keep-alive up and down calls
hrt_call_every(&_keepalivecall_up, 0, (60000 * 1000),
(hrt_callout)&CameraTrigger::keep_alive_up, this);
hrt_call_every(&_keepalivecall_down, 0 + (30000 * 1000), (60000 * 1000),
(hrt_callout)&CameraTrigger::keep_alive_down, this);
} else {
// cancel all calls
hrt_cancel(&_keepalivecall_up);
hrt_cancel(&_keepalivecall_down);
}
}
void
FXAS21002C::stop()
{
hrt_cancel(&_gyro_call);
/* reset internal states */
/* discard unread data in the buffers */
_reports->flush();
}
void
MPU9250::stop()
{
if (_use_hrt) {
hrt_cancel(&_call);
} else {
#ifdef USE_I2C
work_cancel(HPWORK, &_work);
#endif
}
}
int
ORBDevNode::close(struct file *filp)
{
/* is this the publisher closing? */
if (getpid() == _publisher) {
_publisher = 0;
} else {
SubscriberData *sd = filp_to_sd(filp);
if (sd != nullptr) {
hrt_cancel(&sd->update_call);
delete sd;
}
}
return CDev::close(filp);
}
int
uORB::DeviceNode::close(struct file *filp)
{
/* is this the publisher closing? */
if (getpid() == _publisher) {
_publisher = 0;
} else {
SubscriberData *sd = filp_to_sd(filp);
if (sd != nullptr) {
hrt_cancel(&sd->update_call);
remove_internal_subscriber();
delete sd;
sd = nullptr;
}
}
return CDev::close(filp);
}
void
CameraTrigger::stop()
{
work_cancel(LPWORK, &_work);
hrt_cancel(&_engagecall);
hrt_cancel(&_disengagecall);
hrt_cancel(&_engage_turn_on_off_call);
hrt_cancel(&_disengage_turn_on_off_call);
hrt_cancel(&_keepalivecall_up);
hrt_cancel(&_keepalivecall_down);
if (camera_trigger::g_camera_trigger != nullptr) {
delete (camera_trigger::g_camera_trigger);
}
}
void
ToneAlarm::start_tune(const char *tune)
{
// kill any current playback
hrt_cancel(&_note_call);
// record the tune
_tune = tune;
_next = tune;
// initialise player state
_tempo = 120;
_note_length = 4;
_note_mode = MODE_NORMAL;
_octave = 4;
_silence_length = 0;
_repeat = false; // otherwise command-line tunes repeat forever...
// schedule a callback to start playing
hrt_call_after(&_note_call, 0, (hrt_callout)next_trampoline, this);
}
int
uORB::DeviceNode::close(device::file_t *filp)
{
//warnx("uORB::DeviceNode::close fd = %d", filp->fd);
/* is this the publisher closing? */
if (px4_getpid() == _publisher) {
_publisher = 0;
} else {
SubscriberData *sd = filp_to_sd(filp);
if (sd != nullptr) {
hrt_cancel(&sd->update_call);
remove_internal_subscriber();
delete sd;
sd = nullptr;
}
}
return VDev::close(filp);
}
int
ADC::close_last(struct file *filp)
{
hrt_cancel(&_call);
return 0;
}
void
L3G4200D::stop()
{
hrt_cancel(&_call);
}
void
TEST_PPM::stop()
{
hrt_cancel(&_call);
}
void
LSM303D::stop()
{
hrt_cancel(&_accel_call);
hrt_cancel(&_mag_call);
}
void
BMI160::stop()
{
hrt_cancel(&_call);
}
int
ADCSIM::close_last(device::file_t *filp)
{
hrt_cancel(&_call);
return 0;
}
void
BMA180::stop()
{
hrt_cancel(&_call);
}
void
BMI055_accel::stop()
{
hrt_cancel(&_call);
}
void
L3GD20::stop()
{
hrt_cancel(&_call);
}
void
CameraTrigger::cycle_trampoline(void *arg)
{
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
// default loop polling interval
int poll_interval_usec = 5000;
if (trig->_command_sub < 0) {
trig->_command_sub = orb_subscribe(ORB_ID(vehicle_command));
}
bool updated = false;
orb_check(trig->_command_sub, &updated);
struct vehicle_command_s cmd;
unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
bool need_ack = false;
// this flag is set when the polling loop is slowed down to allow the camera to power on
trig->_turning_on = false;
// these flags are used to detect state changes in the command loop
bool main_state = trig->_trigger_enabled;
bool pause_state = trig->_trigger_paused;
// Command handling
if (updated) {
orb_copy(ORB_ID(vehicle_command), trig->_command_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) {
need_ack = true;
if (commandParamToInt(cmd.param7) == 1) {
// test shots are not logged or forwarded to GCS for geotagging
trig->_test_shot = true;
}
if (commandParamToInt((float)cmd.param5) == 1) {
// Schedule shot
trig->_one_shot = true;
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
need_ack = true;
if (commandParamToInt(cmd.param3) == 1) {
// pause triggger
trig->_trigger_paused = true;
} else if (commandParamToInt(cmd.param3) == 0) {
trig->_trigger_paused = false;
}
if (commandParamToInt(cmd.param2) == 1) {
// reset trigger sequence
trig->_trigger_seq = 0;
}
if (commandParamToInt(cmd.param1) == 1) {
trig->_trigger_enabled = true;
} else if (commandParamToInt(cmd.param1) == 0) {
trig->_trigger_enabled = false;
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
need_ack = true;
/*
* TRANSITIONAL SUPPORT ADDED AS OF 11th MAY 2017 (v1.6 RELEASE)
*/
if (cmd.param1 > 0.0f) {
trig->_distance = cmd.param1;
param_set(trig->_p_distance, &(trig->_distance));
trig->_trigger_enabled = true;
trig->_trigger_paused = false;
} else if (commandParamToInt(cmd.param1) == 0) {
trig->_trigger_paused = true;
} else if (commandParamToInt(cmd.param1) == -1) {
trig->_trigger_enabled = false;
}
// We can only control the shutter integration time of the camera in GPIO mode (for now)
if (cmd.param2 > 0.0f) {
if (trig->_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
trig->_activation_time = cmd.param2;
param_set(trig->_p_activation_time, &(trig->_activation_time));
}
}
// Trigger once immediately if param is set
if (cmd.param3 > 0.0f) {
// Schedule shot
trig->_one_shot = true;
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL) {
need_ack = true;
if (cmd.param1 > 0.0f) {
trig->_interval = cmd.param1;
param_set(trig->_p_interval, &(trig->_interval));
}
// We can only control the shutter integration time of the camera in GPIO mode
if (cmd.param2 > 0.0f) {
if (trig->_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
trig->_activation_time = cmd.param2;
param_set(trig->_p_activation_time, &(trig->_activation_time));
}
}
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
}
// State change handling
if ((main_state != trig->_trigger_enabled) ||
(pause_state != trig->_trigger_paused) ||
trig->_one_shot) {
if (trig->_trigger_enabled || trig->_one_shot) { // Just got enabled via a command
// If camera isn't already powered on, we enable power to it
if (!trig->_camera_interface->is_powered_on() &&
trig->_camera_interface->has_power_control()) {
trig->toggle_power();
trig->enable_keep_alive(true);
// Give the camera time to turn on before starting to send trigger signals
poll_interval_usec = 3000000;
trig->_turning_on = true;
}
} else if (!trig->_trigger_enabled || trig->_trigger_paused) { // Just got disabled/paused via a command
// Power off the camera if we are disabled
if (trig->_camera_interface->is_powered_on() &&
trig->_camera_interface->has_power_control() &&
!trig->_trigger_enabled) {
trig->enable_keep_alive(false);
trig->toggle_power();
}
// cancel all calls for both disabled and paused
hrt_cancel(&trig->_engagecall);
hrt_cancel(&trig->_disengagecall);
// ensure that the pin is off
hrt_call_after(&trig->_disengagecall, 0,
(hrt_callout)&CameraTrigger::disengage, trig);
// reset distance counter if needed
if (trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD ||
trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
// this will force distance counter reinit on getting enabled/unpaused
trig->_valid_position = false;
}
}
// only run on state changes, not every loop iteration
if (trig->_trigger_mode == TRIGGER_MODE_INTERVAL_ON_CMD) {
// update intervalometer state and reset calls
trig->update_intervalometer();
}
}
// run every loop iteration and trigger if needed
if (trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD ||
trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {
// update distance counter and trigger
trig->update_distance();
}
// One shot command-based capture handling
if (trig->_one_shot && !trig->_turning_on) {
// One-shot trigger
trig->shoot_once();
trig->_one_shot = false;
if (trig->_test_shot) {
trig->_test_shot = false;
}
}
// Command ACK handling
if (updated && need_ack) {
vehicle_command_ack_s command_ack = {
.timestamp = 0,
.result_param2 = 0,
.command = cmd.command,
.result = (uint8_t)cmd_result,
.from_external = 0,
.result_param1 = 0,
.target_system = cmd.source_system,
.target_component = cmd.source_component
};
if (trig->_cmd_ack_pub == nullptr) {
trig->_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), trig->_cmd_ack_pub, &command_ack);
}
}
work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline,
camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec));
}
void
MPU9250::stop()
{
hrt_cancel(&_call);
}
void
CameraTrigger::cycle_trampoline(void *arg)
{
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
if (trig->_vcommand_sub < 0) {
trig->_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command));
}
bool updated;
orb_check(trig->_vcommand_sub, &updated);
// while the trigger is inactive it has to be ready
// to become active instantaneously
int poll_interval_usec = 5000;
if (trig->_mode < 3) {
if (updated) {
struct vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
// Set trigger rate from command
if (cmd.param2 > 0) {
trig->_interval = cmd.param2;
param_set(trig->_p_interval, &(trig->_interval));
}
if (cmd.param1 < 1.0f) {
trig->control(false);
} else if (cmd.param1 >= 1.0f) {
trig->control(true);
// while the trigger is active there is no
// need to poll at a very high rate
poll_interval_usec = 100000;
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) {
if (cmd.param5 > 0) {
// One-shot trigger, default 1 ms interval
trig->_interval = 1000;
trig->control(true);
}
}
}
} else {
// Set trigger based on covered distance
if (trig->_vlposition_sub < 0) {
trig->_vlposition_sub = orb_subscribe(ORB_ID(vehicle_local_position));
}
struct vehicle_local_position_s pos;
orb_copy(ORB_ID(vehicle_local_position), trig->_vlposition_sub, &pos);
if (pos.xy_valid) {
bool turning_on = false;
if (updated && trig->_mode == 4) {
// Check update from command
struct vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
// Set trigger to disabled if the set distance is not positive
if (cmd.param1 > 0.0f && !trig->_trigger_enabled) {
trig->turnOnOff();
trig->keepAlive(true);
// Give the camera time to turn on, before starting to send trigger signals
poll_interval_usec = 5000000;
turning_on = true;
} else if (cmd.param1 <= 0.0f && trig->_trigger_enabled) {
hrt_cancel(&(trig->_engagecall));
hrt_cancel(&(trig->_disengagecall));
trig->keepAlive(false);
trig->turnOnOff();
}
trig->_trigger_enabled = cmd.param1 > 0.0f;
trig->_distance = cmd.param1;
}
}
if ((trig->_trigger_enabled || trig->_mode < 4) && !turning_on) {
// Initialize position if not done yet
math::Vector<2> current_position(pos.x, pos.y);
if (!trig->_valid_position) {
// First time valid position, take first shot
trig->_last_shoot_position = current_position;
trig->_valid_position = pos.xy_valid;
trig->shootOnce();
}
// Check that distance threshold is exceeded and the time between last shot is large enough
if ((trig->_last_shoot_position - current_position).length() >= trig->_distance) {
trig->shootOnce();
trig->_last_shoot_position = current_position;
}
}
} else {
poll_interval_usec = 100000;
}
}
work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline,
camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec));
}
void
BMI055_gyro::stop()
{
hrt_cancel(&_call);
}
