mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-20 11:28:58 +08:00
[module] add object_tracking module based on FOLLOW_TARGET message
This commit is contained in:
Gautier Hattenberger
@@ -0,0 +1,34 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="object_tracking" dir="ctrl">
|
||||
<doc>
|
||||
<description>
|
||||
Control a rotorcraft heading to track an object detected by a camera
|
||||
|
||||
Data are coming from FOLLOW_TARGET ABI message
|
||||
</description>
|
||||
<section name="OBJECT_TRACKING" prefix="OBJECT_TRACKING_">
|
||||
<define name="ID" value="ABI_BROADCAST" description="ABI binding ID"/>
|
||||
<define name="TIMEOUT" value="2." description="timeout for entering search mode"/>
|
||||
<define name="RATE" value="10." unit="deg/s" description="turn rate during control mode"/>
|
||||
<define name="SEARCH_RATE" value="20." unit="deg/s" description="turn rate during search mode"/>
|
||||
<define name="DEBUG" value="FALSE/TRUE" description="send debug message over telemetry"/>
|
||||
</section>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings NAME="object track">
|
||||
<dl_setting MAX="90." MIN="0." STEP="0.1" VAR="object_tracking_search_rate" shortname="search rate" unit="rad/s" alt_unit="deg/s" module="modules/ctrl/object_tracking"/>
|
||||
<dl_setting MAX="90." MIN="0." STEP="0.1" VAR="object_tracking_rate" shortname="tracking rate" unit="rad/s" alt_unit="deg/s" module="modules/ctrl/object_tracking"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="object_tracking.h"/>
|
||||
</header>
|
||||
<init fun="object_tracking_init()"/>
|
||||
<makefile>
|
||||
<file name="object_tracking.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -0,0 +1,129 @@
|
||||
/*
|
||||
* Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/ctrl/object_tracking.c"
|
||||
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* Control a rotorcraft heading to track an object detected by a camera
|
||||
*/
|
||||
|
||||
#include "modules/ctrl/object_tracking.h"
|
||||
|
||||
#include "firmwares/rotorcraft/navigation.h"
|
||||
#include "subsystems/abi.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "generated/modules.h"
|
||||
|
||||
// ABI message binding ID
|
||||
#ifndef OBJECT_TRACKING_ID
|
||||
#define OBJECT_TRACKING_ID ABI_BROADCAST
|
||||
#endif
|
||||
|
||||
// Timout in seconds before entering search mode
|
||||
#ifndef OBJECT_TRACKING_TIMEOUT
|
||||
#define OBJECT_TRACKING_TIMEOUT 3.0f
|
||||
#endif
|
||||
|
||||
// Turn rate in tracking mode (rad/s)
|
||||
#ifndef OBJECT_TRACKING_RATE
|
||||
#define OBJECT_TRACKING_RATE RadOfDeg(10)
|
||||
#endif
|
||||
|
||||
// Turn rate in search mode (rad/s)
|
||||
#ifndef OBJECT_TRACKING_SEARCH_RATE
|
||||
#define OBJECT_TRACKING_SEARCH_RATE RadOfDeg(20)
|
||||
#endif
|
||||
|
||||
// Send debug message
|
||||
#ifndef OBJECT_TRACKING_DEBUG
|
||||
#define OBJECT_TRACKING_DEBUG FALSE
|
||||
#endif
|
||||
|
||||
#if OBJECT_TRACKING_DEBUG
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
#include "pprzlink/messages.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
#endif
|
||||
|
||||
float object_tracking_rate; // in rad/s
|
||||
float object_tracking_search_rate; // in rad/s
|
||||
|
||||
static uint8_t object_frame; // relative or global coordinates
|
||||
static float object_bearing;
|
||||
static float object_height;
|
||||
static float timeout;
|
||||
|
||||
abi_event object_ev;
|
||||
|
||||
static const float nav_dt = 1.f / NAV_FREQ;
|
||||
|
||||
// callback on follow target message
|
||||
static void get_object(uint8_t sender_id __attribute__((unused)),
|
||||
uint32_t id __attribute__((unused)),
|
||||
uint8_t frame, float bearing, float height,
|
||||
float distance __attribute__((unused)))
|
||||
{
|
||||
object_frame = frame;
|
||||
object_bearing = bearing;
|
||||
object_height = height;
|
||||
timeout = 0.f;
|
||||
}
|
||||
|
||||
void object_tracking_init(void)
|
||||
{
|
||||
object_tracking_search_rate = OBJECT_TRACKING_SEARCH_RATE;
|
||||
object_tracking_rate = OBJECT_TRACKING_RATE;
|
||||
|
||||
object_frame = 0; // relative coordinates
|
||||
object_bearing = 0.f;
|
||||
object_height = 0.f;
|
||||
timeout = OBJECT_TRACKING_TIMEOUT; // start in search mode
|
||||
|
||||
// Bind to camera message
|
||||
AbiBindMsgFOLLOW_TARGET(OBJECT_TRACKING_ID, &object_ev, get_object);
|
||||
}
|
||||
|
||||
void object_tracking_run(void)
|
||||
{
|
||||
if (timeout < OBJECT_TRACKING_TIMEOUT) {
|
||||
timeout += nav_dt;
|
||||
// compute expected heading
|
||||
float target_heading = object_bearing;
|
||||
if (!bit_is_set(object_frame, 0)) {
|
||||
target_heading += stateGetNedToBodyEulers_f()->psi; // relative frame
|
||||
}
|
||||
float diff = target_heading - ANGLE_FLOAT_OF_BFP(nav_heading);
|
||||
FLOAT_ANGLE_NORMALIZE(diff);
|
||||
BoundAbs(diff, object_tracking_rate * nav_dt)
|
||||
nav_heading += ANGLE_BFP_OF_REAL(diff);
|
||||
#if OBJECT_TRACKING_DEBUG
|
||||
float msg[] = {
|
||||
target_heading,
|
||||
object_bearing,
|
||||
object_height,
|
||||
timeout
|
||||
};
|
||||
DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 4, msg);
|
||||
#endif
|
||||
} else {
|
||||
nav_heading += ANGLE_BFP_OF_REAL(object_tracking_search_rate * nav_dt);
|
||||
}
|
||||
INT32_COURSE_NORMALIZE(nav_heading);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,56 @@
|
||||
/*
|
||||
* Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @file "modules/ctrl/object_tracking.h"
|
||||
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* Control a rotorcraft heading to track an object detected by a camera
|
||||
*/
|
||||
|
||||
#ifndef OBJECT_TRACKING_H
|
||||
#define OBJECT_TRACKING_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
/** max turn rate in control mode in rad/s
|
||||
*/
|
||||
extern float object_tracking_rate;
|
||||
|
||||
/** max turn rate in search mode in rad/s
|
||||
*/
|
||||
extern float object_tracking_search_rate;
|
||||
|
||||
/** init function
|
||||
*/
|
||||
extern void object_tracking_init(void);
|
||||
|
||||
/** run function
|
||||
*
|
||||
* should be called in a flight plan stay block using pre_call
|
||||
*
|
||||
* ex:
|
||||
* <block name="Track Object">
|
||||
* <stay wp="STDBY" pre_call="object_tracking_run()"/>
|
||||
* </block>
|
||||
*
|
||||
*/
|
||||
extern void object_tracking_run(void);
|
||||
|
||||
#endif
|
||||
|
||||
Reference in New Issue
Block a user