void
Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result)
{
vehicle_command_ack_s command_ack = {};
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
command_ack.from_external = false;
command_ack.result = result;
command_ack.result_param1 = 0;
command_ack.result_param2 = 0;
if (_vehicle_cmd_ack_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command_ack), _vehicle_cmd_ack_pub, &command_ack);
} else {
_vehicle_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
}
}
void
MissionBlock::issue_command(const struct mission_item_s *item)
{
if (item_contains_position(item)) {
return;
}
if (item->nav_cmd == NAV_CMD_DO_SET_SERVO) {
PX4_INFO("do_set_servo command");
// XXX: we should issue a vehicle command and handle this somewhere else
memset(&actuators, 0, sizeof(actuators));
// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
// params[1] new value for selected actuator in ms 900...2000
actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1];
actuators.timestamp = hrt_absolute_time();
if (_actuator_pub != nullptr) {
orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators);
} else {
_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators);
}
} else {
PX4_INFO("forwarding command %d", item->nav_cmd);
struct vehicle_command_s cmd = {};
mission_item_to_vehicle_command(item, &cmd);
_action_start = hrt_absolute_time();
if (_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
} else {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
}
}
}
int uORBTest::UnitTest::pub_test_queue_main()
{
struct orb_test_medium t;
orb_advert_t ptopic;
const unsigned int queue_size = 50;
t.val = 0;
if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) {
_thread_should_exit = true;
return test_fail("advertise failed: %d", errno);
}
int message_counter = 0, num_messages = 20 * queue_size;
++t.val;
while (message_counter < num_messages) {
//simulate burst
int burst_counter = 0;
while (burst_counter++ < queue_size / 2 + 7) { //make interval non-boundary aligned
orb_publish(ORB_ID(orb_test_medium_queue_poll), ptopic, &t);
++t.val;
}
message_counter += burst_counter;
usleep(20 * 1000); //give subscriber a chance to catch up
}
_num_messages_sent = t.val;
usleep(100 * 1000);
_thread_should_exit = true;
orb_unadvertise(ptopic);
return 0;
}
int uORBTest::UnitTest::test_queue()
{
test_note("Testing orb queuing");
struct orb_test_medium t, u;
int sfd;
orb_advert_t ptopic;
bool updated;
sfd = orb_subscribe(ORB_ID(orb_test_medium_queue));
if (sfd < 0) {
return test_fail("subscribe failed: %d", errno);
}
const unsigned int queue_size = 11;
t.val = 0;
ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size);
if (ptopic == nullptr) {
return test_fail("advertise failed: %d", errno);
}
orb_check(sfd, &updated);
if (!updated) {
return test_fail("update flag not set");
}
if (PX4_OK != orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u)) {
return test_fail("copy(1) failed: %d", errno);
}
if (u.val != t.val) {
return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val);
}
orb_check(sfd, &updated);
if (updated) {
return test_fail("spurious updated flag");
}
#define CHECK_UPDATED(element) \
orb_check(sfd, &updated); \
if (!updated) { \
return test_fail("update flag not set, element %i", element); \
}
#define CHECK_NOT_UPDATED(element) \
orb_check(sfd, &updated); \
if (updated) { \
return test_fail("update flag set, element %i", element); \
}
#define CHECK_COPY(i_got, i_correct) \
orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u); \
if (i_got != i_correct) { \
return test_fail("got wrong element from the queue (got %i, should be %i)", i_got, i_correct); \
}
//no messages in the queue anymore
test_note(" Testing to write some elements...");
for (unsigned int i = 0; i < queue_size - 2; ++i) {
t.val = i;
orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
}
for (unsigned int i = 0; i < queue_size - 2; ++i) {
CHECK_UPDATED(i);
CHECK_COPY(u.val, i);
}
CHECK_NOT_UPDATED(queue_size);
test_note(" Testing overflow...");
int overflow_by = 3;
for (unsigned int i = 0; i < queue_size + overflow_by; ++i) {
t.val = i;
orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
}
for (unsigned int i = 0; i < queue_size; ++i) {
CHECK_UPDATED(i);
CHECK_COPY(u.val, i + overflow_by);
}
CHECK_NOT_UPDATED(queue_size);
test_note(" Testing underflow...");
for (unsigned int i = 0; i < queue_size; ++i) {
CHECK_NOT_UPDATED(i);
CHECK_COPY(u.val, queue_size + overflow_by - 1);
}
t.val = 943;
orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t);
CHECK_UPDATED(-1);
CHECK_COPY(u.val, t.val);
#undef CHECK_COPY
#undef CHECK_UPDATED
#undef CHECK_NOT_UPDATED
orb_unadvertise(ptopic);
return test_note("PASS orb queuing");
}
void FlightTasks::_updateCommand()
{
// lazy subscription to command topic
if (_sub_vehicle_command < 0) {
_sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
}
// check if there's any new command
bool updated = false;
orb_check(_sub_vehicle_command, &updated);
if (!updated) {
return;
}
// get command
struct vehicle_command_s command;
orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command);
// check what command it is
FlightTaskIndex desired_task = switchVehicleCommand(command.command);
if (desired_task == FlightTaskIndex::None) {
// ignore all unkown commands
return;
}
// switch to the commanded task
int switch_result = switchTask(desired_task);
uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
// if we are in/switched to the desired task
if (switch_result >= 0) {
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
// if the task is running apply parameters to it and see if it rejects
if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
// if we just switched and parameters are not accepted, go to failsafe
if (switch_result == 1) {
switchTask(FlightTaskIndex::ManualPosition);
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
}
}
}
// send back acknowledgment
vehicle_command_ack_s command_ack = {};
command_ack.command = command.command;
command_ack.result = cmd_result;
command_ack.result_param1 = switch_result;
command_ack.target_system = command.source_system;
command_ack.target_component = command.source_component;
if (_pub_vehicle_command_ack == nullptr) {
_pub_vehicle_command_ack = 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), _pub_vehicle_command_ack, &command_ack);
}
}
void
Navigator::task_main()
{
bool have_geofence_position_data = false;
/* Try to load the geofence:
* if /fs/microsd/etc/geofence.txt load from this file
* else clear geofence data in datamanager */
struct stat buffer;
if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
PX4_INFO("Try to load geofence.txt");
_geofence.loadFromFile(GEOFENCE_FILENAME);
} else {
if (_geofence.clearDm() != OK) {
mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence");
}
}
/* do subscriptions */
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
/* copy all topics first time */
vehicle_status_update();
vehicle_land_detected_update();
global_position_update();
local_position_update();
gps_position_update();
sensor_combined_update();
home_position_update(true);
fw_pos_ctrl_status_update();
params_update();
/* wakeup source(s) */
px4_pollfd_struct_t fds[1] = {};
/* Setup of loop */
fds[0].fd = _global_pos_sub;
fds[0].events = POLLIN;
bool global_pos_available_once = false;
/* rate-limit global pos subscription to 20 Hz / 50 ms */
orb_set_interval(_global_pos_sub, 49);
while (!_task_should_exit) {
/* wait for up to 1000ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
if (pret == 0) {
/* timed out - periodic check for _task_should_exit, etc. */
if (global_pos_available_once) {
global_pos_available_once = false;
PX4_WARN("global position timeout");
}
/* Let the loop run anyway, don't do `continue` here. */
} else if (pret < 0) {
/* this is undesirable but not much we can do - might want to flag unhappy status */
PX4_ERR("nav: poll error %d, %d", pret, errno);
usleep(10000);
continue;
} else {
if (fds[0].revents & POLLIN) {
/* success, global pos is available */
global_position_update();
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
have_geofence_position_data = true;
}
global_pos_available_once = true;
}
}
perf_begin(_loop_perf);
bool updated;
/* gps updated */
orb_check(_gps_pos_sub, &updated);
if (updated) {
gps_position_update();
if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
have_geofence_position_data = true;
}
}
/* local position updated */
orb_check(_local_pos_sub, &updated);
if (updated) {
local_position_update();
}
/* sensors combined updated */
orb_check(_sensor_combined_sub, &updated);
if (updated) {
sensor_combined_update();
}
/* parameters updated */
orb_check(_param_update_sub, &updated);
if (updated) {
params_update();
}
/* vehicle status updated */
orb_check(_vstatus_sub, &updated);
if (updated) {
vehicle_status_update();
}
/* vehicle land detected updated */
orb_check(_land_detected_sub, &updated);
if (updated) {
vehicle_land_detected_update();
}
/* navigation capabilities updated */
orb_check(_fw_pos_ctrl_status_sub, &updated);
if (updated) {
fw_pos_ctrl_status_update();
}
/* home position updated */
orb_check(_home_pos_sub, &updated);
if (updated) {
home_position_update();
}
orb_check(_vehicle_command_sub, &updated);
if (updated) {
vehicle_command_s cmd = {};
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
struct position_setpoint_triplet_s *rep = get_reposition_triplet();
// store current position as previous position and goal as next
rep->previous.yaw = get_global_position()->yaw;
rep->previous.lat = get_global_position()->lat;
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;
rep->current.loiter_radius = get_loiter_radius();
rep->current.loiter_direction = 1;
rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
// Go on and check which changes had been requested
if (PX4_ISFINITE(cmd.param4)) {
rep->current.yaw = cmd.param4;
} else {
rep->current.yaw = NAN;
}
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
} else {
rep->current.lat = get_global_position()->lat;
rep->current.lon = get_global_position()->lon;
}
if (PX4_ISFINITE(cmd.param7)) {
rep->current.alt = cmd.param7;
} else {
rep->current.alt = get_global_position()->alt;
}
rep->previous.valid = true;
rep->current.valid = true;
rep->next.valid = false;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
struct position_setpoint_triplet_s *rep = get_takeoff_triplet();
// store current position as previous position and goal as next
rep->previous.yaw = get_global_position()->yaw;
rep->previous.lat = get_global_position()->lat;
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;
rep->current.loiter_radius = get_loiter_radius();
rep->current.loiter_direction = 1;
rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
if (home_position_valid()) {
rep->current.yaw = cmd.param4;
rep->previous.valid = true;
} else {
rep->current.yaw = get_local_position()->yaw;
rep->previous.valid = false;
}
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
} else {
// If one of them is non-finite, reset both
rep->current.lat = NAN;
rep->current.lon = NAN;
}
rep->current.alt = cmd.param7;
rep->current.valid = true;
rep->next.valid = false;
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) {
/* find NAV_CMD_DO_LAND_START in the mission and
* use MAV_CMD_MISSION_START to start the mission there
*/
int land_start = _mission.find_offboard_land_start();
if (land_start != -1) {
vehicle_command_s vcmd = {};
vcmd.target_system = get_vstatus()->system_id;
vcmd.target_component = get_vstatus()->component_id;
vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
vcmd.param1 = land_start;
vcmd.param2 = 0;
publish_vehicle_cmd(vcmd);
} else {
PX4_WARN("planned landing not available");
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH) {
vehicle_command_s vcmd = {};
vcmd.target_system = get_vstatus()->system_id;
vcmd.target_component = get_vstatus()->component_id;
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH;
orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
(void)orb_unadvertise(pub);
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
if (get_mission_result()->valid &&
PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) {
_mission.set_current_offboard_mission_index(cmd.param1);
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
warnx("navigator: got pause/continue command");
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
if (cmd.param2 > FLT_EPSILON) {
// XXX not differentiating ground and airspeed yet
set_cruising_speed(cmd.param2);
} else {
set_cruising_speed();
/* if no speed target was given try to set throttle */
if (cmd.param3 > FLT_EPSILON) {
set_cruising_throttle(cmd.param3 / 100);
} else {
set_cruising_throttle();
}
}
}
}
/* Check geofence violation */
static hrt_abstime last_geofence_check = 0;
if (have_geofence_position_data &&
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos,
home_position_valid());
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
_geofence_result.timestamp = hrt_absolute_time();
_geofence_result.geofence_action = _geofence.getGeofenceAction();
if (!inside) {
/* inform other apps via the mission result */
_geofence_result.geofence_violated = true;
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent) {
mavlink_log_critical(&_mavlink_log_pub, "Geofence violation");
_geofence_violation_warning_sent = true;
}
} else {
/* inform other apps via the mission result */
_geofence_result.geofence_violated = false;
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
publish_geofence_result();
}
/* Do stuff according to navigation state set by commander */
switch (_vstatus.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_mission;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_loiter;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_rcLoss;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_rtl;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_takeoff;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_land;
break;
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_land;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_dataLinkLoss;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_engineFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_gpsFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_follow_target;
break;
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
case vehicle_status_s::NAVIGATION_STATE_STAB:
default:
_pos_sp_triplet_published_invalid_once = false;
_navigation_mode = nullptr;
_can_loiter_at_sp = false;
break;
}
/* iterate through navigation modes and set active/inactive for each */
for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
}
/* if nothing is running, set position setpoint triplet invalid once */
if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
_pos_sp_triplet_published_invalid_once = true;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
_pos_sp_triplet_updated = true;
}
if (_pos_sp_triplet_updated) {
_pos_sp_triplet.timestamp = hrt_absolute_time();
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = false;
}
if (_mission_result_updated) {
publish_mission_result();
_mission_result_updated = false;
}
perf_end(_loop_perf);
}
PX4_INFO("exiting");
_navigator_task = -1;
}
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));
}
