diff --git a/boards/aerotenna/ocpoc/default.cmake b/boards/aerotenna/ocpoc/default.cmake index edbb876a475..7ea73100764 100644 --- a/boards/aerotenna/ocpoc/default.cmake +++ b/boards/aerotenna/ocpoc/default.cmake @@ -11,6 +11,7 @@ px4_add_board( #barometer # all available barometer drivers barometer/ms5611 batt_smbus + camera_capture camera_trigger differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index 5260d52cc34..28db936667f 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -15,7 +15,7 @@ px4_add_board( px4fmu MODULES attitude_estimator_q - camera_feedback + #camera_feedback commander dataman ekf2 diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake index 7a17ac8b240..31bbf30bf5b 100644 --- a/boards/emlid/navio2/cross.cmake +++ b/boards/emlid/navio2/cross.cmake @@ -11,6 +11,7 @@ px4_add_board( #barometer # all available barometer drivers barometer/ms5611 batt_smbus + camera_capture camera_trigger differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake index 8c226b4db6d..12e3b9bc746 100644 --- a/boards/emlid/navio2/native.cmake +++ b/boards/emlid/navio2/native.cmake @@ -9,6 +9,7 @@ px4_add_board( #barometer # all available barometer drivers barometer/ms5611 batt_smbus + camera_capture camera_trigger differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index fbef96bd343..9d4c307d928 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -18,6 +18,7 @@ px4_add_board( barometer # all available barometer drivers barometer/mpl3115a2 batt_smbus + camera_capture camera_trigger differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers diff --git a/boards/parrot/bebop/default.cmake b/boards/parrot/bebop/default.cmake index 7e5094a626f..0a9ecb06445 100644 --- a/boards/parrot/bebop/default.cmake +++ b/boards/parrot/bebop/default.cmake @@ -21,7 +21,7 @@ px4_add_board( MODULES attitude_estimator_q - camera_feedback + #camera_feedback commander dataman ekf2 diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 40b855f970d..970ac1cf950 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -66,7 +66,7 @@ px4_add_board( airspeed_selector #attitude_estimator_q battery_status - camera_feedback + #camera_feedback commander dataman ekf2 diff --git a/boards/px4/fmu-v2/fixedwing.cmake b/boards/px4/fmu-v2/fixedwing.cmake index 622caf636db..4acd86c2104 100644 --- a/boards/px4/fmu-v2/fixedwing.cmake +++ b/boards/px4/fmu-v2/fixedwing.cmake @@ -21,8 +21,8 @@ px4_add_board( #barometer # all available barometer drivers barometer/ms5611 #batt_smbus - #camera_capture - #camera_trigger + camera_capture + camera_trigger differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers gps diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index 68f941780a6..0bb8e283ee1 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -19,8 +19,8 @@ px4_add_board( adc barometer/ms5611 #batt_smbus - #camera_capture - #camera_trigger + camera_capture + camera_trigger distance_sensor # all available distance sensor drivers gps imu/l3gd20 diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index 8fdf9686480..3f98365cc41 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -65,7 +65,7 @@ px4_add_board( #airspeed_selector #attitude_estimator_q battery_status - camera_feedback + #camera_feedback commander dataman #ekf2 diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 6dd460174de..1cbbec4581e 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -60,7 +60,7 @@ px4_add_board( airspeed_selector attitude_estimator_q battery_status - camera_feedback + #camera_feedback commander dataman ekf2 diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 666654726e4..298538be92e 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -8,6 +8,7 @@ px4_add_board( DRIVERS #barometer # all available barometer drivers #batt_smbus + camera_capture camera_trigger #differential_pressure # all available differential pressure drivers #distance_sensor # all available distance sensor drivers diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index 6a02e9e75ea..5c457019126 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -8,6 +8,7 @@ px4_add_board( DRIVERS #barometer # all available barometer drivers #batt_smbus + camera_capture camera_trigger #differential_pressure # all available differential pressure drivers #distance_sensor # all available distance sensor drivers diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index 589f0c88fd0..d2ad12e8a93 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -9,6 +9,7 @@ px4_add_board( DRIVERS #barometer # all available barometer drivers #batt_smbus + camera_capture camera_trigger #differential_pressure # all available differential pressure drivers #distance_sensor # all available distance sensor drivers diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 19f895e2cac..8fe24dd18f5 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -48,6 +48,8 @@ namespace camera_capture CameraCapture *g_camera_capture{nullptr}; } +struct work_s CameraCapture::_work_publisher; + CameraCapture::CameraCapture() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { @@ -67,9 +69,7 @@ CameraCapture::CameraCapture() : CameraCapture::~CameraCapture() { /* free any existing reports */ - if (_trig_buffer != nullptr) { - delete _trig_buffer; - } + delete _trig_buffer; camera_capture::g_camera_capture = nullptr; } @@ -229,8 +229,8 @@ CameraCapture::set_capture_control(bool enabled) conf.edge = Both; } - conf.callback = NULL; - conf.context = NULL; + conf.callback = nullptr; + conf.context = nullptr; if (enabled) { @@ -278,8 +278,6 @@ CameraCapture::set_capture_control(bool enabled) err_out: ::close(fd); #endif - - return; } void @@ -328,7 +326,7 @@ CameraCapture::status() { PX4_INFO("Capture enabled : %s", _capture_enabled ? "YES" : "NO"); PX4_INFO("Frame sequence : %u", _capture_seq); - PX4_INFO("Last trigger timestamp : %llu", _last_trig_time); + PX4_INFO("Last trigger timestamp : %" PRIu64 "", _last_trig_time); if (_camera_capture_mode != 0) { PX4_INFO("Last exposure time : %0.2f ms", double(_last_exposure_time) / 1000.0); diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp index 5d3a4b5ddce..a54275006af 100644 --- a/src/drivers/camera_capture/camera_capture.hpp +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -149,5 +149,3 @@ private: static void publish_trigger_trampoline(void *arg); }; - -struct work_s CameraCapture::_work_publisher; diff --git a/src/modules/camera_feedback/CMakeLists.txt b/src/modules/camera_feedback/CMakeLists.txt index cd168449fe9..152f2c1eae5 100644 --- a/src/modules/camera_feedback/CMakeLists.txt +++ b/src/modules/camera_feedback/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -33,9 +33,9 @@ px4_add_module( MODULE modules__camera_feedback MAIN camera_feedback - COMPILE_FLAGS SRCS - camera_feedback.cpp + CameraFeedback.cpp + CameraFeedback.hpp DEPENDS + px4_work_queue ) - diff --git a/src/modules/camera_feedback/CameraFeedback.cpp b/src/modules/camera_feedback/CameraFeedback.cpp new file mode 100644 index 00000000000..0b81fc79974 --- /dev/null +++ b/src/modules/camera_feedback/CameraFeedback.cpp @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "CameraFeedback.hpp" + +CameraFeedback::CameraFeedback() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ +} + +bool +CameraFeedback::init() +{ + if (!_trigger_sub.registerCallback()) { + PX4_ERR("camera_trigger callback registration failed!"); + return false; + } + + return true; +} + +void +CameraFeedback::Run() +{ + if (should_exit()) { + _trigger_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + camera_trigger_s trig{}; + + if (_trigger_sub.update(&trig)) { + + // update geotagging subscriptions + vehicle_global_position_s gpos{}; + _gpos_sub.copy(&gpos); + + vehicle_attitude_s att{}; + _att_sub.copy(&att); + + if (trig.timestamp == 0 || + gpos.timestamp == 0 || + att.timestamp == 0) { + + // reject until we have valid data + return; + } + + camera_capture_s capture{}; + + // Fill timestamps + capture.timestamp = trig.timestamp; + capture.timestamp_utc = trig.timestamp_utc; + + // Fill image sequence + capture.seq = trig.seq; + + // Fill position data + capture.lat = gpos.lat; + capture.lon = gpos.lon; + capture.alt = gpos.alt; + + if (gpos.terrain_alt_valid) { + capture.ground_distance = gpos.alt - gpos.terrain_alt; + + } else { + capture.ground_distance = -1.0f; + } + + // Fill attitude data + // TODO : this needs to be rotated by camera orientation or set to gimbal orientation when available + capture.q[0] = att.q[0]; + capture.q[1] = att.q[1]; + capture.q[2] = att.q[2]; + capture.q[3] = att.q[3]; + + // Indicate whether capture feedback from camera is available + // What is case 0 for capture.result? + if (!_param_camera_capture_feedback.get()) { + capture.result = -1; + + } else { + capture.result = 1; + } + + _capture_pub.publish(capture); + } +} + +int +CameraFeedback::task_spawn(int argc, char *argv[]) +{ + CameraFeedback *instance = new CameraFeedback(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int +CameraFeedback::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int +CameraFeedback::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("camera_feedback", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int camera_feedback_main(int argc, char *argv[]) +{ + return CameraFeedback::main(argc, argv); +} diff --git a/src/modules/camera_feedback/camera_feedback.hpp b/src/modules/camera_feedback/CameraFeedback.hpp similarity index 61% rename from src/modules/camera_feedback/camera_feedback.hpp rename to src/modules/camera_feedback/CameraFeedback.hpp index aac70a4a89d..a08642c31d3 100644 --- a/src/modules/camera_feedback/camera_feedback.hpp +++ b/src/modules/camera_feedback/CameraFeedback.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,81 +32,60 @@ ****************************************************************************/ /** - * @file camera_feedback.hpp * + * Online and offline geotagging from camera feedback + * + * @author Mohammed Kabir */ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include - +#include +#include #include #include -#include +#include +#include #include -#include - -#include -#include +#include +#include +#include +#include #include +#include #include -#include #include - -class CameraFeedback +class CameraFeedback : public ModuleBase, public ModuleParams, public px4::WorkItem { public: - /** - * Constructor - */ CameraFeedback(); + ~CameraFeedback() override = default; - /** - * Destructor, also kills task. - */ - ~CameraFeedback(); + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); - /** - * Start the task. - * - * @return OK on success. - */ - int start(); + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); - /** - * Stop the task. - */ - void stop(); + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + void Run() override; + + bool init(); private: - bool _task_should_exit; /**< if true, task should exit */ - int _main_task; /**< handle for task */ + uORB::SubscriptionCallbackWorkItem _trigger_sub{this, ORB_ID(camera_trigger)}; - int _trigger_sub; - int _gpos_sub; - int _att_sub; + uORB::Subscription _gpos_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; - orb_advert_t _capture_pub; + uORB::Publication _capture_pub{ORB_ID(camera_capture)}; - param_t _p_camera_capture_feedback; - - int32_t _camera_capture_feedback; - - void task_main(); - - /** - * Shim for calling task_main from task_create. - */ - static int task_main_trampoline(int argc, char *argv[]); + DEFINE_PARAMETERS( + (ParamBool) _param_camera_capture_feedback + ) }; diff --git a/src/modules/camera_feedback/camera_feedback.cpp b/src/modules/camera_feedback/camera_feedback.cpp deleted file mode 100644 index 4e3fb0ce9f1..00000000000 --- a/src/modules/camera_feedback/camera_feedback.cpp +++ /dev/null @@ -1,282 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file camera_feedback.cpp - * - * Online and offline geotagging from camera feedback - * - * @author Mohammed Kabir - */ - -#include "camera_feedback.hpp" - -namespace camera_feedback -{ -CameraFeedback *g_camera_feedback; -} - -CameraFeedback::CameraFeedback() : - _task_should_exit(false), - _main_task(-1), - _trigger_sub(-1), - _gpos_sub(-1), - _att_sub(-1), - _capture_pub(nullptr), - _camera_capture_feedback(false) -{ - - // Parameters - _p_camera_capture_feedback = param_find("CAM_CAP_FBACK"); - - param_get(_p_camera_capture_feedback, (int32_t *)&_camera_capture_feedback); - -} - -CameraFeedback::~CameraFeedback() -{ - - if (_main_task != -1) { - - /* task wakes up every 100ms or so at the longest */ - _task_should_exit = true; - - /* wait for a second for the task to quit at our request */ - unsigned i = 0; - - do { - /* wait 20ms */ - px4_usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - px4_task_delete(_main_task); - break; - } - } while (_main_task != -1); - } - - camera_feedback::g_camera_feedback = nullptr; -} - -int -CameraFeedback::start() -{ - - /* start the task */ - _main_task = px4_task_spawn_cmd("camera_feedback", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT + 15, - 1600, - (px4_main_t)&CameraFeedback::task_main_trampoline, - nullptr); - - if (_main_task < 0) { - warn("task start failed"); - return -errno; - } - - return OK; - -} - -void -CameraFeedback::stop() -{ - if (camera_feedback::g_camera_feedback != nullptr) { - delete (camera_feedback::g_camera_feedback); - } -} - - -void -CameraFeedback::task_main() -{ - _trigger_sub = orb_subscribe(ORB_ID(camera_trigger)); - - // Polling sources - struct camera_trigger_s trig = {}; - - px4_pollfd_struct_t fds[1] = {}; - fds[0].fd = _trigger_sub; - fds[0].events = POLLIN; - - // Geotagging subscriptions - _gpos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - struct vehicle_global_position_s gpos = {}; - struct vehicle_attitude_s att = {}; - - bool updated = false; - - while (!_task_should_exit) { - - /* wait for up to 20ms for data */ - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 20); - - if (pret < 0) { - PX4_WARN("poll error %d, %d", pret, errno); - continue; - } - - /* trigger subscription updated */ - if (fds[0].revents & POLLIN) { - - orb_copy(ORB_ID(camera_trigger), _trigger_sub, &trig); - - /* update geotagging subscriptions */ - orb_check(_gpos_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_global_position), _gpos_sub, &gpos); - } - - orb_check(_att_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); - } - - if (trig.timestamp == 0 || - gpos.timestamp == 0 || - att.timestamp == 0) { - // reject until we have valid data - continue; - } - - struct camera_capture_s capture = {}; - - // Fill timestamps - capture.timestamp = trig.timestamp; - - capture.timestamp_utc = trig.timestamp_utc; - - // Fill image sequence - capture.seq = trig.seq; - - // Fill position data - capture.lat = gpos.lat; - - capture.lon = gpos.lon; - - capture.alt = gpos.alt; - - capture.ground_distance = gpos.terrain_alt_valid ? (gpos.alt - gpos.terrain_alt) : -1.0f; - - // Fill attitude data - // TODO : this needs to be rotated by camera orientation or set to gimbal orientation when available - capture.q[0] = att.q[0]; - - capture.q[1] = att.q[1]; - - capture.q[2] = att.q[2]; - - capture.q[3] = att.q[3]; - - // Indicate whether capture feedback from camera is available - // What is case 0 for capture.result? - if (!_camera_capture_feedback) { - capture.result = -1; - - } else { - capture.result = 1; - } - - int instance_id; - - orb_publish_auto(ORB_ID(camera_capture), &_capture_pub, &capture, &instance_id, ORB_PRIO_DEFAULT); - - } - - } - - orb_unsubscribe(_trigger_sub); - orb_unsubscribe(_gpos_sub); - orb_unsubscribe(_att_sub); - - _main_task = -1; - -} - -int -CameraFeedback::task_main_trampoline(int argc, char *argv[]) -{ - camera_feedback::g_camera_feedback->task_main(); - return 0; -} - -static int usage() -{ - PX4_INFO("usage: camera_feedback {start|stop}\n"); - return 1; -} - -extern "C" __EXPORT int camera_feedback_main(int argc, char *argv[]); - -int camera_feedback_main(int argc, char *argv[]) -{ - if (argc < 2) { - return usage(); - } - - if (!strcmp(argv[1], "start")) { - - if (camera_feedback::g_camera_feedback != nullptr) { - PX4_WARN("already running"); - return 0; - } - - camera_feedback::g_camera_feedback = new CameraFeedback(); - - if (camera_feedback::g_camera_feedback == nullptr) { - PX4_WARN("alloc failed"); - return 1; - } - - camera_feedback::g_camera_feedback->start(); - return 0; - } - - if (camera_feedback::g_camera_feedback == nullptr) { - PX4_WARN("not running"); - return 1; - - } else if (!strcmp(argv[1], "stop")) { - camera_feedback::g_camera_feedback->stop(); - - } else { - return usage(); - } - - return 0; -}