mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-02 22:20:34 +08:00
segway move to examples
This commit is contained in:
Daniel Agar
committed by
Lorenz Meier
Lorenz Meier
parent
89ff9f1fe3
commit
0611677ee2
@@ -1,60 +0,0 @@
|
||||
#include "BlockSegwayController.hpp"
|
||||
|
||||
void BlockSegwayController::update()
|
||||
{
|
||||
// wait for a sensor update, check for exit condition every 100 ms
|
||||
if (px4_poll(&_attPoll, 1, 100) < 0) { return; } // poll error
|
||||
|
||||
uint64_t newTimeStamp = hrt_absolute_time();
|
||||
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
|
||||
_timeStamp = newTimeStamp;
|
||||
|
||||
// check for sane values of dt
|
||||
// to prevent large control responses
|
||||
if (dt > 1.0f || dt < 0) { return; }
|
||||
|
||||
// set dt for all child blocks
|
||||
setDt(dt);
|
||||
|
||||
// check for new updates
|
||||
if (_param_update.updated()) { updateParams(); }
|
||||
|
||||
// get new information from subscriptions
|
||||
updateSubscriptions();
|
||||
|
||||
// default all output to zero unless handled by mode
|
||||
for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++) {
|
||||
_actuators.control[i] = 0.0f;
|
||||
}
|
||||
|
||||
// only update guidance in auto mode
|
||||
if (_status.main_state == MAIN_STATE_AUTO) {
|
||||
// update guidance
|
||||
}
|
||||
|
||||
// compute speed command
|
||||
float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);
|
||||
|
||||
// handle autopilot modes
|
||||
if (_status.main_state == MAIN_STATE_AUTO ||
|
||||
_status.main_state == MAIN_STATE_ALTCTL ||
|
||||
_status.main_state == MAIN_STATE_POSCTL) {
|
||||
_actuators.control[0] = spdCmd;
|
||||
_actuators.control[1] = spdCmd;
|
||||
|
||||
} else if (_status.main_state == MAIN_STATE_MANUAL) {
|
||||
if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {
|
||||
_actuators.control[CH_LEFT] = _manual.throttle;
|
||||
_actuators.control[CH_RIGHT] = _manual.pitch;
|
||||
|
||||
} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {
|
||||
_actuators.control[0] = spdCmd;
|
||||
_actuators.control[1] = spdCmd;
|
||||
}
|
||||
}
|
||||
|
||||
// update all publications
|
||||
updatePublications();
|
||||
|
||||
}
|
||||
|
||||
@@ -1,29 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <px4_posix.h>
|
||||
#include <controllib/uorb/blocks.hpp>
|
||||
|
||||
using namespace control;
|
||||
|
||||
class BlockSegwayController : public control::BlockUorbEnabledAutopilot
|
||||
{
|
||||
public:
|
||||
BlockSegwayController() :
|
||||
BlockUorbEnabledAutopilot(NULL, "SEG"),
|
||||
th2v(this, "TH2V"),
|
||||
q2v(this, "Q2V"),
|
||||
_attPoll(),
|
||||
_timeStamp(0)
|
||||
{
|
||||
_attPoll.fd = _att.getHandle();
|
||||
_attPoll.events = POLLIN;
|
||||
}
|
||||
void update();
|
||||
private:
|
||||
enum {CH_LEFT, CH_RIGHT};
|
||||
BlockPI th2v;
|
||||
BlockP q2v;
|
||||
px4_pollfd_struct_t _attPoll;
|
||||
uint64_t _timeStamp;
|
||||
};
|
||||
|
||||
@@ -1,43 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015 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.
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE modules__segway
|
||||
MAIN segway
|
||||
SRCS
|
||||
segway_main.cpp
|
||||
BlockSegwayController.cpp
|
||||
params.c
|
||||
DEPENDS
|
||||
platforms__common
|
||||
)
|
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
|
||||
@@ -1,8 +0,0 @@
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
// 16 is max name length
|
||||
PARAM_DEFINE_FLOAT(SEG_TH2V_P, 10.0f); // pitch to voltage
|
||||
PARAM_DEFINE_FLOAT(SEG_TH2V_I, 0.0f); // pitch integral to voltage
|
||||
PARAM_DEFINE_FLOAT(SEG_TH2V_I_MAX, 0.0f); // integral limiter
|
||||
PARAM_DEFINE_FLOAT(SEG_Q2V, 1.0f); // pitch rate to voltage
|
||||
|
||||
@@ -1,159 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: James Goppert
|
||||
*
|
||||
* 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 segway_main.cpp
|
||||
* @author James Goppert
|
||||
*
|
||||
* Segway controller using control library
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "BlockSegwayController.hpp"
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
/**
|
||||
* Deamon management function.
|
||||
*/
|
||||
extern "C" __EXPORT int segway_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Mainloop of deamon.
|
||||
*/
|
||||
int segway_thread_main(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
}
|
||||
|
||||
fprintf(stderr, "usage: segway {start|stop|status} [-p <additional params>]\n\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The deamon app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int segway_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc < 2) {
|
||||
usage("missing command");
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
warnx("already running");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
|
||||
deamon_task = px4_task_spawn_cmd("segway",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 10,
|
||||
5120,
|
||||
segway_thread_main,
|
||||
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
warnx("is running");
|
||||
|
||||
} else {
|
||||
warnx("not started");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int segway_thread_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
warnx("starting");
|
||||
|
||||
using namespace control;
|
||||
|
||||
BlockSegwayController autopilot;
|
||||
|
||||
thread_running = true;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
autopilot.update();
|
||||
}
|
||||
|
||||
warnx("exiting.");
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -37,20 +37,20 @@
|
||||
*/
|
||||
|
||||
#include "Publication.hpp"
|
||||
#include "topics/vehicle_attitude.h"
|
||||
#include "topics/vehicle_local_position.h"
|
||||
#include "topics/vehicle_global_position.h"
|
||||
#include "topics/debug_key_value.h"
|
||||
#include "topics/actuator_controls.h"
|
||||
#include "topics/vehicle_global_velocity_setpoint.h"
|
||||
#include "topics/vehicle_attitude_setpoint.h"
|
||||
#include "topics/vehicle_rates_setpoint.h"
|
||||
#include "topics/actuator_outputs.h"
|
||||
#include "topics/actuator_direct.h"
|
||||
#include "topics/tecs_status.h"
|
||||
#include "topics/rc_channels.h"
|
||||
#include "topics/filtered_bottom_flow.h"
|
||||
#include "topics/actuator_outputs.h"
|
||||
#include "topics/debug_key_value.h"
|
||||
#include "topics/ekf2_innovations.h"
|
||||
#include "topics/filtered_bottom_flow.h"
|
||||
#include "topics/rc_channels.h"
|
||||
#include "topics/tecs_status.h"
|
||||
#include "topics/vehicle_attitude.h"
|
||||
#include "topics/vehicle_attitude_setpoint.h"
|
||||
#include "topics/vehicle_global_position.h"
|
||||
#include "topics/vehicle_global_velocity_setpoint.h"
|
||||
#include "topics/vehicle_local_position.h"
|
||||
#include "topics/vehicle_rates_setpoint.h"
|
||||
|
||||
#include <px4_defines.h>
|
||||
|
||||
@@ -108,19 +108,19 @@ PublicationNode::PublicationNode(const struct orb_metadata *meta,
|
||||
}
|
||||
|
||||
// explicit template instantiation
|
||||
template class __EXPORT Publication<vehicle_attitude_s>;
|
||||
template class __EXPORT Publication<vehicle_local_position_s>;
|
||||
template class __EXPORT Publication<vehicle_global_position_s>;
|
||||
template class __EXPORT Publication<debug_key_value_s>;
|
||||
template class __EXPORT Publication<actuator_controls_s>;
|
||||
template class __EXPORT Publication<vehicle_global_velocity_setpoint_s>;
|
||||
template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
|
||||
template class __EXPORT Publication<vehicle_rates_setpoint_s>;
|
||||
template class __EXPORT Publication<actuator_outputs_s>;
|
||||
template class __EXPORT Publication<actuator_direct_s>;
|
||||
template class __EXPORT Publication<tecs_status_s>;
|
||||
template class __EXPORT Publication<rc_channels_s>;
|
||||
template class __EXPORT Publication<filtered_bottom_flow_s>;
|
||||
template class __EXPORT Publication<actuator_outputs_s>;
|
||||
template class __EXPORT Publication<debug_key_value_s>;
|
||||
template class __EXPORT Publication<ekf2_innovations_s>;
|
||||
template class __EXPORT Publication<filtered_bottom_flow_s>;
|
||||
template class __EXPORT Publication<rc_channels_s>;
|
||||
template class __EXPORT Publication<tecs_status_s>;
|
||||
template class __EXPORT Publication<vehicle_attitude_s>;
|
||||
template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
|
||||
template class __EXPORT Publication<vehicle_global_position_s>;
|
||||
template class __EXPORT Publication<vehicle_global_velocity_setpoint_s>;
|
||||
template class __EXPORT Publication<vehicle_local_position_s>;
|
||||
template class __EXPORT Publication<vehicle_rates_setpoint_s>;
|
||||
|
||||
}
|
||||
|
||||
@@ -117,9 +117,7 @@ public:
|
||||
* @param list A list interface for adding to
|
||||
* list during construction
|
||||
*/
|
||||
PublicationNode(const struct orb_metadata *meta,
|
||||
int priority = -1,
|
||||
List<PublicationNode *> *list = nullptr);
|
||||
PublicationNode(const struct orb_metadata *meta, int priority = -1, List<PublicationNode *> *list = nullptr);
|
||||
|
||||
/**
|
||||
* This function is the callback for list traversal
|
||||
@@ -145,9 +143,7 @@ public:
|
||||
* @param list A list interface for adding to
|
||||
* list during construction
|
||||
*/
|
||||
Publication(const struct orb_metadata *meta,
|
||||
int priority = -1,
|
||||
List<PublicationNode *> *list = nullptr) :
|
||||
Publication(const struct orb_metadata *meta, int priority = -1, List<PublicationNode *> *list = nullptr) :
|
||||
PublicationNode(meta, priority, list),
|
||||
_data()
|
||||
{
|
||||
|
||||
@@ -37,33 +37,33 @@
|
||||
*/
|
||||
|
||||
#include "Subscription.hpp"
|
||||
#include "topics/parameter_update.h"
|
||||
#include "topics/actuator_armed.h"
|
||||
#include "topics/actuator_controls.h"
|
||||
#include "topics/vehicle_gps_position.h"
|
||||
#include "topics/satellite_info.h"
|
||||
#include "topics/sensor_combined.h"
|
||||
#include "topics/att_pos_mocap.h"
|
||||
#include "topics/battery_status.h"
|
||||
#include "topics/control_state.h"
|
||||
#include "topics/distance_sensor.h"
|
||||
#include "topics/hil_sensor.h"
|
||||
#include "topics/vehicle_attitude.h"
|
||||
#include "topics/vehicle_global_position.h"
|
||||
#include "topics/position_setpoint_triplet.h"
|
||||
#include "topics/vehicle_status.h"
|
||||
#include "topics/home_position.h"
|
||||
#include "topics/log_message.h"
|
||||
#include "topics/manual_control_setpoint.h"
|
||||
#include "topics/mavlink_log.h"
|
||||
#include "topics/log_message.h"
|
||||
#include "topics/vehicle_local_position_setpoint.h"
|
||||
#include "topics/vehicle_local_position.h"
|
||||
#include "topics/vehicle_attitude_setpoint.h"
|
||||
#include "topics/vehicle_rates_setpoint.h"
|
||||
#include "topics/rc_channels.h"
|
||||
#include "topics/battery_status.h"
|
||||
#include "topics/optical_flow.h"
|
||||
#include "topics/distance_sensor.h"
|
||||
#include "topics/home_position.h"
|
||||
#include "topics/parameter_update.h"
|
||||
#include "topics/position_setpoint_triplet.h"
|
||||
#include "topics/rc_channels.h"
|
||||
#include "topics/satellite_info.h"
|
||||
#include "topics/sensor_combined.h"
|
||||
#include "topics/vehicle_attitude.h"
|
||||
#include "topics/vehicle_attitude_setpoint.h"
|
||||
#include "topics/vehicle_control_mode.h"
|
||||
#include "topics/actuator_armed.h"
|
||||
#include "topics/att_pos_mocap.h"
|
||||
#include "topics/control_state.h"
|
||||
#include "topics/vehicle_global_position.h"
|
||||
#include "topics/vehicle_gps_position.h"
|
||||
#include "topics/vehicle_land_detected.h"
|
||||
#include "topics/vehicle_local_position.h"
|
||||
#include "topics/vehicle_local_position_setpoint.h"
|
||||
#include "topics/vehicle_rates_setpoint.h"
|
||||
#include "topics/vehicle_status.h"
|
||||
|
||||
#include <px4_defines.h>
|
||||
|
||||
@@ -154,32 +154,32 @@ bool Subscription<T>::check_updated()
|
||||
template <class T>
|
||||
const T &Subscription<T>::get() { return _data; }
|
||||
|
||||
template class __EXPORT Subscription<parameter_update_s>;
|
||||
template class __EXPORT Subscription<actuator_armed_s>;
|
||||
template class __EXPORT Subscription<actuator_controls_s>;
|
||||
template class __EXPORT Subscription<vehicle_gps_position_s>;
|
||||
template class __EXPORT Subscription<satellite_info_s>;
|
||||
template class __EXPORT Subscription<sensor_combined_s>;
|
||||
template class __EXPORT Subscription<att_pos_mocap_s>;
|
||||
template class __EXPORT Subscription<battery_status_s>;
|
||||
template class __EXPORT Subscription<control_state_s>;
|
||||
template class __EXPORT Subscription<distance_sensor_s>;
|
||||
template class __EXPORT Subscription<hil_sensor_s>;
|
||||
template class __EXPORT Subscription<vehicle_attitude_s>;
|
||||
template class __EXPORT Subscription<vehicle_global_position_s>;
|
||||
template class __EXPORT Subscription<position_setpoint_triplet_s>;
|
||||
template class __EXPORT Subscription<vehicle_status_s>;
|
||||
template class __EXPORT Subscription<home_position_s>;
|
||||
template class __EXPORT Subscription<log_message_s>;
|
||||
template class __EXPORT Subscription<manual_control_setpoint_s>;
|
||||
template class __EXPORT Subscription<mavlink_log_s>;
|
||||
template class __EXPORT Subscription<log_message_s>;
|
||||
template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
|
||||
template class __EXPORT Subscription<vehicle_local_position_s>;
|
||||
template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
|
||||
template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
|
||||
template class __EXPORT Subscription<rc_channels_s>;
|
||||
template class __EXPORT Subscription<vehicle_control_mode_s>;
|
||||
template class __EXPORT Subscription<actuator_armed_s>;
|
||||
template class __EXPORT Subscription<battery_status_s>;
|
||||
template class __EXPORT Subscription<home_position_s>;
|
||||
template class __EXPORT Subscription<optical_flow_s>;
|
||||
template class __EXPORT Subscription<distance_sensor_s>;
|
||||
template class __EXPORT Subscription<att_pos_mocap_s>;
|
||||
template class __EXPORT Subscription<control_state_s>;
|
||||
template class __EXPORT Subscription<parameter_update_s>;
|
||||
template class __EXPORT Subscription<position_setpoint_triplet_s>;
|
||||
template class __EXPORT Subscription<rc_channels_s>;
|
||||
template class __EXPORT Subscription<satellite_info_s>;
|
||||
template class __EXPORT Subscription<sensor_combined_s>;
|
||||
template class __EXPORT Subscription<vehicle_attitude_s>;
|
||||
template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
|
||||
template class __EXPORT Subscription<vehicle_control_mode_s>;
|
||||
template class __EXPORT Subscription<vehicle_global_position_s>;
|
||||
template class __EXPORT Subscription<vehicle_gps_position_s>;
|
||||
template class __EXPORT Subscription<vehicle_land_detected_s>;
|
||||
template class __EXPORT Subscription<vehicle_local_position_s>;
|
||||
template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
|
||||
template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
|
||||
template class __EXPORT Subscription<vehicle_status_s>;
|
||||
|
||||
} // namespace uORB
|
||||
|
||||
Reference in New Issue
Block a user