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

segway move to examples

This commit is contained in:
Daniel Agar 2017-02-25 13:51:38 -05:00 committed by Lorenz Meier
parent 89ff9f1fe3
commit 0611677ee2
15 changed files with 187 additions and 159 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cmake/configs/nuttx_px4fmu-v3_default.cmake
View File

@ -170,6 +170,11 @@ set(config_module_list
#
examples/rover_steering_control
#
# Segway
#
examples/segway
#
# Demo apps
#

5
cmake/configs/nuttx_px4fmu-v4pro_default.cmake
View File

@ -171,6 +171,11 @@ set(config_module_list
#
examples/rover_steering_control
#
# Segway
#
#examples/segway
#
# Demo apps
#

5
cmake/configs/nuttx_px4fmu-v5_default.cmake
View File

@ -169,6 +169,11 @@ set(config_module_list
#
#examples/rover_steering_control
#
# Segway
#
#examples/segway
#
# Demo apps
#

5
cmake/configs/posix_sitl_default.cmake
View File

@ -138,6 +138,11 @@ set(config_module_list
#
examples/rover_steering_control
#
# Segway
#
examples/segway
#
# Demo apps
#

66
src/examples/segway/BlockSegwayController.cpp Normal file
View File

@ -0,0 +1,66 @@
#include "BlockSegwayController.hpp"
using matrix::Eulerf;
using matrix::Quatf;
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();
actuator_controls_s &actuators = _actuators.get();
// default all output to zero unless handled by mode
for (unsigned i = 2; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
const uint8_t &nav_state = _status.get().nav_state;
// only update guidance in auto mode
if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
// update guidance
}
Eulerf euler = Eulerf(Quatf(_att.get().q));
// compute speed command
float spdCmd = -th2v.update(euler.theta()) - q2v.update(_att.get().pitchspeed);
// handle autopilot modes
if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL ||
nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL) {
actuators.control[0] = spdCmd;
actuators.control[1] = spdCmd;
} else if (nav_state == vehicle_status_s::NAVIGATION_STATE_STAB) {
actuators.control[0] = spdCmd;
actuators.control[1] = spdCmd;
} else if (nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL) {
actuators.control[CH_LEFT] = _manual.get().z;
actuators.control[CH_RIGHT] = -_manual.get().x;
}
// update all publications
updatePublications();
}

9
src/modules/segway/BlockSegwayController.hpp → src/examples/segway/BlockSegwayController.hpp
View File

@ -1,9 +1,11 @@
#pragma once
#include <px4_posix.h>
#include <controllib/blocks.hpp>
#include <controllib/uorb/blocks.hpp>
using namespace control;
using control::BlockPI;
using control::BlockP;
class BlockSegwayController : public control::BlockUorbEnabledAutopilot
{
@ -18,12 +20,15 @@ public:
_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;
};

6
src/modules/segway/CMakeLists.txt → src/examples/segway/CMakeLists.txt
View File

@ -31,8 +31,10 @@
#
############################################################################
px4_add_module(
MODULE modules__segway
MODULE examples__segway
MAIN segway
STACK_MAIN 1300
STACK_MAX 1300
SRCS
segway_main.cpp
BlockSegwayController.cpp
@ -40,4 +42,4 @@ px4_add_module(
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
# vim: set noet ft=cmake fenc=utf-8 ff=unix :

1
src/modules/segway/params.c → src/examples/segway/params.c
View File

@ -5,4 +5,3 @@ 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

0
src/modules/segway/segway_main.cpp → src/examples/segway/segway_main.cpp
View File

13
src/lib/controllib/uorb/blocks.cpp
View File

@ -84,20 +84,19 @@ void BlockWaypointGuidance::update(
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_manual(ORB_ID(manual_control_setpoint), 20, 0, &getSubscriptions()),
_param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz
_missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()),
_att(ORB_ID(vehicle_attitude), 20, 0, &getSubscriptions()),
_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, 0, &getSubscriptions()),
_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()),
_pos(ORB_ID(vehicle_global_position), 20, 0, &getSubscriptions()),
_missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()),
_manual(ORB_ID(manual_control_setpoint), 20, 0, &getSubscriptions()),
_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()),
_status(ORB_ID(vehicle_status), 20, 0, &getSubscriptions()),
_param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz
// publications
_actuators(ORB_ID(actuator_controls_0), -1, &getPublications())
{
}
BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
} // namespace control
}

37
src/lib/controllib/uorb/blocks.hpp
View File

@ -39,27 +39,27 @@
#pragma once
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <math.h>
#include <poll.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <controllib/blocks.hpp>
#include <drivers/drv_hrt.h>
#include <poll.h>
#include "../blocks.hpp"
#include <uORB/Subscription.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_controls.h>
namespace control
{
@ -90,19 +90,20 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
{
protected:
// subscriptions
uORB::Subscription<manual_control_setpoint_s> _manual;
uORB::Subscription<parameter_update_s> _param_update;
uORB::Subscription<position_setpoint_triplet_s> _missionCmd;
uORB::Subscription<vehicle_attitude_s> _att;
uORB::Subscription<vehicle_attitude_setpoint_s> _attCmd;
uORB::Subscription<vehicle_rates_setpoint_s> _ratesCmd;
uORB::Subscription<vehicle_global_position_s> _pos;
uORB::Subscription<position_setpoint_triplet_s> _missionCmd;
uORB::Subscription<manual_control_setpoint_s> _manual;
uORB::Subscription<vehicle_rates_setpoint_s> _ratesCmd;
uORB::Subscription<vehicle_status_s> _status;
uORB::Subscription<parameter_update_s> _param_update;
// publications
uORB::Publication<actuator_controls_s> _actuators;
public:
BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
virtual ~BlockUorbEnabledAutopilot();
virtual ~BlockUorbEnabledAutopilot() = default;
};
} // namespace control

60
src/modules/segway/BlockSegwayController.cpp
View File

@ -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();
}

44
src/modules/uORB/Publication.cpp
View File

@ -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>;
}

8
src/modules/uORB/Publication.hpp
View File

@ -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()
{

82
src/modules/uORB/Subscription.cpp
View File

@ -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