void
CameraTrigger::shootOnce()
{
// schedule trigger on and off calls
hrt_call_after(&_engagecall, 0,
(hrt_callout)&CameraTrigger::engage, this);
// schedule trigger on and off calls
hrt_call_after(&_disengagecall, 0 + (_activation_time * 1000),
(hrt_callout)&CameraTrigger::disengage, this);
}
void
CameraTrigger::turnOnOff()
{
// schedule trigger on and off calls
hrt_call_after(&_engage_turn_on_off_call, 0,
(hrt_callout)&CameraTrigger::engange_turn_on_off, this);
// schedule trigger on and off calls
hrt_call_after(&_disengage_turn_on_off_call, 0 + (200 * 1000),
(hrt_callout)&CameraTrigger::disengage_turn_on_off, this);
}
void
CameraTrigger::toggle_power()
{
// schedule power toggle calls
hrt_call_after(&_engage_turn_on_off_call, 0,
(hrt_callout)&CameraTrigger::engange_turn_on_off, this);
hrt_call_after(&_disengage_turn_on_off_call, 0 + (200 * 1000),
(hrt_callout)&CameraTrigger::disengage_turn_on_off, this);
}
void
TEST_PPM::do_out(void)
{
if ((_call_times % 2) == 0) {
px4_arch_gpiowrite(TEST_PPM_PIN, false);
hrt_call_after(&_call, _values[_call_times / 2] - _plus_width, (hrt_callout)&TEST_PPM::loops, this);
} else {
px4_arch_gpiowrite(TEST_PPM_PIN, true);
hrt_call_after(&_call, _plus_width, (hrt_callout)&TEST_PPM::loops, this);
}
if ((_call_times / 2) < _channels + 1) { _call_times++; }
else { _call_times = 0; }
}
void
TEST_PPM::start()
{
stop();
_call_times = 0;
hrt_call_after(&_call, 1000, (hrt_callout)&TEST_PPM::loops, this);
}
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
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 test_hrt(int argc, char *argv[])
{
struct hrt_call call;
hrt_abstime prev, now;
int i;
struct timeval tv1, tv2;
printf("start-time (hrt, sec/usec), end-time (hrt, sec/usec), microseconds per 1/10 second\n");
for (i = 0; i < 10; i++) {
prev = hrt_absolute_time();
gettimeofday(&tv1, nullptr);
usleep(100000);
now = hrt_absolute_time();
gettimeofday(&tv2, nullptr);
printf("%lu (%lu/%lu), %lu (%lu/%lu), %lu\n",
(unsigned long)prev, (unsigned long)tv1.tv_sec, (unsigned long)tv1.tv_usec,
(unsigned long)now, (unsigned long)tv2.tv_sec, (unsigned long)tv2.tv_usec,
(unsigned long)(hrt_absolute_time() - prev));
fflush(stdout);
}
usleep(1000000);
printf("one-second ticks\n");
for (i = 0; i < 3; i++) {
hrt_call_after(&call, 1000000, nullptr, nullptr);
while (!hrt_called(&call)) {
usleep(1000);
}
printf("tick\n");
fflush(stdout);
}
return 0;
}
void
ToneAlarm::next_note()
{
// do we have an inter-note gap to wait for?
if (_silence_length > 0) {
stop_note();
hrt_call_after(&_note_call, (hrt_abstime)_silence_length, (hrt_callout)next_trampoline, this);
_silence_length = 0;
return;
}
// make sure we still have a tune - may be removed by the write / ioctl handler
if ((_next == nullptr) || (_tune == nullptr)) {
stop_note();
return;
}
// parse characters out of the string until we have resolved a note
unsigned note = 0;
unsigned note_length = _note_length;
unsigned duration;
while (note == 0) {
// we always need at least one character from the string
int c = next_char();
if (c == 0) {
goto tune_end;
}
_next++;
switch (c) {
case 'L': // select note length
_note_length = next_number();
if (_note_length < 1) {
goto tune_error;
}
break;
case 'O': // select octave
_octave = next_number();
if (_octave > 6) {
_octave = 6;
}
break;
case '<': // decrease octave
if (_octave > 0) {
_octave--;
}
break;
case '>': // increase octave
if (_octave < 6) {
_octave++;
}
break;
case 'M': // select inter-note gap
c = next_char();
if (c == 0) {
goto tune_error;
}
_next++;
switch (c) {
case 'N':
_note_mode = MODE_NORMAL;
break;
case 'L':
_note_mode = MODE_LEGATO;
break;
case 'S':
_note_mode = MODE_STACCATO;
break;
case 'F':
_repeat = false;
break;
case 'B':
_repeat = true;
break;
default:
goto tune_error;
}
break;
case 'P': // pause for a note length
stop_note();
hrt_call_after(&_note_call,
(hrt_abstime)rest_duration(next_number(), next_dots()),
(hrt_callout)next_trampoline,
this);
return;
case 'T': { // change tempo
unsigned nt = next_number();
if ((nt >= 32) && (nt <= 255)) {
_tempo = nt;
} else {
goto tune_error;
}
break;
}
case 'N': // play an arbitrary note
note = next_number();
if (note > 84) {
goto tune_error;
}
if (note == 0) {
// this is a rest - pause for the current note length
hrt_call_after(&_note_call,
(hrt_abstime)rest_duration(_note_length, next_dots()),
(hrt_callout)next_trampoline,
this);
return;
}
break;
case 'A'...'G': // play a note in the current octave
note = _note_tab[c - 'A'] + (_octave * 12) + 1;
c = next_char();
switch (c) {
case '#': // up a semitone
case '+':
if (note < 84) {
note++;
}
_next++;
break;
case '-': // down a semitone
if (note > 1) {
note--;
}
_next++;
break;
default:
// 0 / no next char here is OK
break;
}
// shorthand length notation
note_length = next_number();
if (note_length == 0) {
note_length = _note_length;
}
break;
default:
goto tune_error;
}
}
// compute the duration of the note and the following silence (if any)
duration = note_duration(_silence_length, note_length, next_dots());
// start playing the note
start_note(note);
// and arrange a callback when the note should stop
hrt_call_after(&_note_call, (hrt_abstime)duration, (hrt_callout)next_trampoline, this);
return;
// tune looks bad (unexpected EOF, bad character, etc.)
tune_error:
syslog(LOG_ERR, "tune error\n");
_repeat = false; // don't loop on error
// stop (and potentially restart) the tune
tune_end:
stop_note();
if (_repeat) {
start_tune(_tune);
} else {
_tune = nullptr;
_default_tune_number = 0;
}
return;
}
// XXX refactor this out of this driver
void
PWMIN::hard_reset()
{
_turn_off();
hrt_call_after(&_hard_reset_call, 9000, reinterpret_cast<hrt_callout>(&PWMIN::_turn_on), this);
}
bool
ORBDevNode::appears_updated(SubscriberData *sd)
{
/* assume it doesn't look updated */
bool ret = false;
/* avoid racing between interrupt and non-interrupt context calls */
irqstate_t state = irqsave();
/* check if this topic has been published yet, if not bail out */
if (_data == nullptr) {
ret = false;
goto out;
}
/*
* If the subscriber's generation count matches the update generation
* count, there has been no update from their perspective; if they
* don't match then we might have a visible update.
*/
while (sd->generation != _generation) {
/*
* Handle non-rate-limited subscribers.
*/
if (sd->update_interval == 0) {
ret = true;
break;
}
/*
* If we have previously told the subscriber that there is data,
* and they have not yet collected it, continue to tell them
* that there has been an update. This mimics the non-rate-limited
* behaviour where checking / polling continues to report an update
* until the topic is read.
*/
if (sd->update_reported) {
ret = true;
break;
}
/*
* If the interval timer is still running, the topic should not
* appear updated, even though at this point we know that it has.
* We have previously been through here, so the subscriber
* must have collected the update we reported, otherwise
* update_reported would still be true.
*/
if (!hrt_called(&sd->update_call))
break;
/*
* Make sure that we don't consider the topic to be updated again
* until the interval has passed once more by restarting the interval
* timer and thereby re-scheduling a poll notification at that time.
*/
hrt_call_after(&sd->update_call,
sd->update_interval,
&ORBDevNode::update_deferred_trampoline,
(void *)this);
/*
* Remember that we have told the subscriber that there is data.
*/
sd->update_reported = true;
ret = true;
break;
}
out:
irqrestore(state);
/* consider it updated */
return ret;
}
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
CameraTrigger::poll(void *arg)
{
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
bool updated;
orb_check(trig->_vcommand_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &trig->_command);
if(trig->_command.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL)
{
if(trig->_command.param1 < 1)
{
if(trig->_trigger_enabled)
{
trig->_trigger_enabled = false ;
}
}
else if(trig->_command.param1 >= 1)
{
if(!trig->_trigger_enabled)
{
trig->_trigger_enabled = true ;
}
}
// Set trigger rate from command
if(trig->_command.param2 > 0)
{
trig->_integration_time = trig->_command.param2;
param_set(trig->integration_time, &(trig->_integration_time));
}
}
}
if(!trig->_trigger_enabled) {
hrt_call_after(&trig->_pollcall, 1e6, (hrt_callout)&CameraTrigger::poll, trig);
return;
}
else
{
engage(trig);
hrt_call_after(&trig->_firecall, trig->_activation_time*1000, (hrt_callout)&CameraTrigger::disengage, trig);
orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor);
trig->_trigger.timestamp = trig->_sensor.timestamp; // get IMU timestamp
trig->_trigger.seq = trig->_trigger_seq++;
if (trig->_trigger_pub != nullptr) {
orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trig->_trigger);
} else {
trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger);
}
hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time)*1000, (hrt_callout)&CameraTrigger::poll, trig);
}
}
