mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Camera trigger: fix formatting
This commit is contained in:
Lorenz Meier
parent
a02319e901
commit
65d035a892
@ -89,21 +89,21 @@ public:
|
||||
* Display info.
|
||||
*/
|
||||
void info();
|
||||
|
||||
|
||||
int pin;
|
||||
|
||||
|
||||
private:
|
||||
|
||||
|
||||
struct hrt_call _pollcall;
|
||||
struct hrt_call _firecall;
|
||||
|
||||
|
||||
int _gpio_fd;
|
||||
|
||||
int _polarity;
|
||||
float _activation_time;
|
||||
float _integration_time;
|
||||
float _transfer_time;
|
||||
uint32_t _trigger_seq;
|
||||
uint32_t _trigger_seq;
|
||||
bool _trigger_enabled;
|
||||
|
||||
int _sensor_sub;
|
||||
@ -116,10 +116,10 @@ private:
|
||||
struct vehicle_command_s _command;
|
||||
|
||||
param_t polarity ;
|
||||
param_t activation_time ;
|
||||
param_t activation_time ;
|
||||
param_t integration_time ;
|
||||
param_t transfer_time ;
|
||||
|
||||
|
||||
/**
|
||||
* Topic poller to check for fire info.
|
||||
*/
|
||||
@ -162,13 +162,13 @@ CameraTrigger::CameraTrigger() :
|
||||
memset(&_trigger, 0, sizeof(_trigger));
|
||||
memset(&_sensor, 0, sizeof(_sensor));
|
||||
memset(&_command, 0, sizeof(_command));
|
||||
|
||||
|
||||
memset(&_pollcall, 0, sizeof(_pollcall));
|
||||
memset(&_firecall, 0, sizeof(_firecall));
|
||||
memset(&_firecall, 0, sizeof(_firecall));
|
||||
|
||||
// Parameters
|
||||
polarity = param_find("TRIG_POLARITY");
|
||||
activation_time = param_find("TRIG_ACT_TIME");
|
||||
activation_time = param_find("TRIG_ACT_TIME");
|
||||
integration_time = param_find("TRIG_INT_TIME");
|
||||
transfer_time = param_find("TRIG_TRANS_TIME");
|
||||
}
|
||||
@ -184,37 +184,37 @@ CameraTrigger::start()
|
||||
|
||||
_sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
|
||||
param_get(polarity, &_polarity);
|
||||
param_get(activation_time, &_activation_time);
|
||||
param_get(integration_time, &_integration_time);
|
||||
param_get(transfer_time, &_transfer_time);
|
||||
|
||||
|
||||
param_get(polarity, &_polarity);
|
||||
param_get(activation_time, &_activation_time);
|
||||
param_get(integration_time, &_integration_time);
|
||||
param_get(transfer_time, &_transfer_time);
|
||||
|
||||
stm32_configgpio(GPIO_GPIO0_OUTPUT);
|
||||
|
||||
if(_polarity == 0) {
|
||||
if (_polarity == 0) {
|
||||
stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1); // GPIO pin pull high
|
||||
}
|
||||
else if(_polarity == 1) {
|
||||
|
||||
} else if (_polarity == 1) {
|
||||
stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0); // GPIO pin pull low
|
||||
}
|
||||
else {
|
||||
|
||||
} else {
|
||||
warnx(" invalid trigger polarity setting. stopping.");
|
||||
stop();
|
||||
}
|
||||
|
||||
|
||||
poll(this); // Trampoline call
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
CameraTrigger::stop()
|
||||
{
|
||||
{
|
||||
hrt_cancel(&_firecall);
|
||||
hrt_cancel(&_pollcall);
|
||||
|
||||
if (camera_trigger::g_camera_trigger != nullptr) {
|
||||
delete (camera_trigger::g_camera_trigger);
|
||||
delete(camera_trigger::g_camera_trigger);
|
||||
}
|
||||
}
|
||||
|
||||
@ -226,60 +226,55 @@ CameraTrigger::poll(void *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 ;
|
||||
|
||||
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)
|
||||
{
|
||||
|
||||
} else if (trig->_command.param1 >= 1) {
|
||||
if (!trig->_trigger_enabled) {
|
||||
trig->_trigger_enabled = true ;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Set trigger rate from command
|
||||
if(trig->_command.param2 > 0)
|
||||
{
|
||||
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);
|
||||
if (!trig->_trigger_enabled) {
|
||||
hrt_call_after(&trig->_pollcall, 1e6, (hrt_callout)&CameraTrigger::poll, trig);
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
} else {
|
||||
engage(trig);
|
||||
hrt_call_after(&trig->_firecall, trig->_activation_time*1000, (hrt_callout)&CameraTrigger::disengage, 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);
|
||||
hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time) * 1000,
|
||||
(hrt_callout)&CameraTrigger::poll, trig);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
@ -289,17 +284,15 @@ CameraTrigger::engage(void *arg)
|
||||
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
|
||||
|
||||
stm32_configgpio(GPIO_GPIO0_OUTPUT);
|
||||
|
||||
if(trig->_polarity == 0) // ACTIVE_LOW
|
||||
{
|
||||
stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0);
|
||||
|
||||
if (trig->_polarity == 0) { // ACTIVE_LOW
|
||||
stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0);
|
||||
|
||||
} else if (trig->_polarity == 1) { // ACTIVE_HIGH
|
||||
stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1);
|
||||
}
|
||||
else if(trig->_polarity == 1) // ACTIVE_HIGH
|
||||
{
|
||||
stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
@ -307,18 +300,16 @@ CameraTrigger::disengage(void *arg)
|
||||
{
|
||||
|
||||
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
|
||||
|
||||
|
||||
stm32_configgpio(GPIO_GPIO0_OUTPUT);
|
||||
|
||||
if(trig->_polarity == 0) // ACTIVE_LOW
|
||||
{
|
||||
stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1);
|
||||
|
||||
if (trig->_polarity == 0) { // ACTIVE_LOW
|
||||
stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 1);
|
||||
|
||||
} else if (trig->_polarity == 1) { // ACTIVE_HIGH
|
||||
stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0);
|
||||
}
|
||||
else if(trig->_polarity == 1) // ACTIVE_HIGH
|
||||
{
|
||||
stm32_gpiowrite(GPIO_GPIO0_OUTPUT, 0);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
@ -333,8 +324,8 @@ CameraTrigger::info()
|
||||
static void usage()
|
||||
{
|
||||
errx(1, "usage: camera_trigger {start|stop|info} [-p <n>]\n"
|
||||
"\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
|
||||
);
|
||||
"\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
|
||||
);
|
||||
}
|
||||
|
||||
int camera_trigger_main(int argc, char *argv[])
|
||||
@ -348,25 +339,26 @@ int camera_trigger_main(int argc, char *argv[])
|
||||
if (camera_trigger::g_camera_trigger != nullptr) {
|
||||
errx(0, "already running");
|
||||
}
|
||||
|
||||
|
||||
camera_trigger::g_camera_trigger = new CameraTrigger;
|
||||
|
||||
if (camera_trigger::g_camera_trigger == nullptr) {
|
||||
errx(1, "alloc failed");
|
||||
}
|
||||
|
||||
|
||||
if (argc > 3) {
|
||||
|
||||
|
||||
camera_trigger::g_camera_trigger->pin = (int)argv[3];
|
||||
if (atoi(argv[3]) > 0 && atoi(argv[3]) < 6) {
|
||||
warnx("starting trigger on pin : %li ", atoi(argv[3]));
|
||||
|
||||
if (atoi(argv[3]) > 0 && atoi(argv[3]) < 6) {
|
||||
warnx("starting trigger on pin : %li ", atoi(argv[3]));
|
||||
camera_trigger::g_camera_trigger->pin = atoi(argv[3]);
|
||||
}
|
||||
else
|
||||
{
|
||||
usage();
|
||||
|
||||
} else {
|
||||
usage();
|
||||
}
|
||||
}
|
||||
|
||||
camera_trigger::g_camera_trigger->start();
|
||||
|
||||
return 0;
|
||||
@ -377,11 +369,10 @@ int camera_trigger_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
else if (!strcmp(argv[1], "stop")) {
|
||||
camera_trigger::g_camera_trigger->stop();
|
||||
camera_trigger::g_camera_trigger->stop();
|
||||
|
||||
}
|
||||
else if (!strcmp(argv[1], "info")) {
|
||||
camera_trigger::g_camera_trigger->info();
|
||||
} else if (!strcmp(argv[1], "info")) {
|
||||
camera_trigger::g_camera_trigger->info();
|
||||
|
||||
} else {
|
||||
usage();
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user