dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fixed launchdetection logic, catapult tested in HIL

This commit is contained in:
Thomas Gubler 2013-12-30 19:08:09 +01:00
parent bd88209f5c
commit 9cc1fc1cb5
2 changed files with 48 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/lib/launchdetection/CatapultLaunchMethod.cpp
View File

@ -39,6 +39,7 @@
*/
#include "CatapultLaunchMethod.h"
#include <systemlib/err.h>
CatapultLaunchMethod::CatapultLaunchMethod() :
last_timestamp(0),
@ -61,11 +62,15 @@ void CatapultLaunchMethod::update(float accel_x)
if (accel_x > threshold_accel.get()) {
integrator += accel_x * dt;
// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt);
if (integrator > threshold_accel.get() * threshold_time.get()) {
launchDetected = true;
}
} else {
// warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt);
/* reset integrator */
integrator = 0.0f;
launchDetected = false;

67
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -76,6 +76,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@ -132,7 +133,7 @@ private:
int _control_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _accel_sub; /**< body frame accelerations */
int _sensor_combined_sub; /**< for body frame accelerations */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */
@ -145,7 +146,7 @@ private:
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */
struct accel_report _accel; /**< body frame accelerations */
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
perf_counter_t _loop_perf; /**< loop performance counter */
@ -171,6 +172,10 @@ private:
bool land_motor_lim;
bool land_onslope;
/* takeoff/launch states */
bool launch_detected;
bool launch_detection_message_sent;
/* Landingslope object */
Landingslope landingslope;
@ -311,7 +316,7 @@ private:
/**
* Check for accel updates.
*/
void vehicle_accel_poll();
void vehicle_sensor_combined_poll();
/**
* Check for set triplet updates.
@ -389,7 +394,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
land_onslope(false),
flare_curve_alt_last(0.0f),
_mavlink_fd(-1),
launchDetector()
launchDetector(),
launch_detected(false),
launch_detection_message_sent(false)
{
/* safely initialize structs */
vehicle_attitude_s _att = {0};
@ -400,7 +407,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
vehicle_control_mode_s _control_mode = {0};
vehicle_global_position_s _global_pos = {0};
mission_item_triplet_s _mission_item_triplet = {0};
accel_report _accel = {0};
sensor_combined_s _sensor_combined = {0};
@ -631,14 +638,14 @@ FixedwingPositionControl::vehicle_attitude_poll()
}
void
FixedwingPositionControl::vehicle_accel_poll()
FixedwingPositionControl::vehicle_sensor_combined_poll()
{
/* check if there is a new position */
bool accel_updated;
orb_check(_accel_sub, &accel_updated);
bool sensors_updated;
orb_check(_sensor_combined_sub, &sensors_updated);
if (accel_updated) {
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
if (sensors_updated) {
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
}
}
@ -756,7 +763,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
float baro_altitude = _global_pos.alt;
/* filter speed and altitude for controller */
math::Vector3 accel_body(_accel.x, _accel.y, _accel.z);
math::Vector3 accel_body(_sensor_combined.accelerometer_m_s2[0], _sensor_combined.accelerometer_m_s2[1], _sensor_combined.accelerometer_m_s2[2]);
math::Vector3 accel_earth = _R_nb.transpose() * accel_body;
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
@ -973,24 +980,30 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
} else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
/* Perform launch detection */
bool do_fly_takeoff = false;
warnx("Launch detection running");
if (launchDetector.launchDetectionEnabled()) {
launchDetector.update(_accel.x);
if (launchDetector.getLaunchDetected()) {
do_fly_takeoff = true;
warnx("Launch detected. Taking off!");
// warnx("Launch detection running");
if(!launch_detected) { //do not do further checks once a launch was detected
if (launchDetector.launchDetectionEnabled()) {
// warnx("Launch detection enabled");
if(!launch_detection_message_sent) {
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
launch_detection_message_sent = true;
}
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
if (launchDetector.getLaunchDetected()) {
launch_detected = true;
warnx("Launch detected. Taking off!");
}
} else {
/* no takeoff detection --> fly */
launch_detected = true;
}
} else {
/* no takeoff detection --> fly */
do_fly_takeoff = true;
}
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
if (do_fly_takeoff) {
if (launch_detected) {
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
if (altitude_error > 15.0f) {
@ -1037,6 +1050,12 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
land_onslope = false;
}
/* reset takeoff/launch state */
if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) {
launch_detected = false;
launch_detection_message_sent = false;
}
if (was_circle_mode && !_l1_control.circle_mode()) {
/* just kicked out of loiter, reset roll integrals */
_att_sp.roll_reset_integral = true;
@ -1151,7 +1170,7 @@ FixedwingPositionControl::task_main()
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
@ -1233,7 +1252,7 @@ FixedwingPositionControl::task_main()
vehicle_attitude_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
// vehicle_baro_poll();