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

Added controller parameters, added vicon position reading

This commit is contained in:
Lorenz Meier 2012-10-04 15:33:39 +02:00
parent 607e902b88
commit 67a2c8a173
6 changed files with 262 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
apps/mavlink/mavlink.c
View File

@ -68,6 +68,7 @@
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/optical_flow.h>
@ -134,6 +135,8 @@ static struct vehicle_command_s vcmd;
static struct actuator_armed_s armed;
static struct vehicle_vicon_position_s vicon_position;
static orb_advert_t pub_hil_global_pos = -1;
static orb_advert_t cmd_pub = -1;
static orb_advert_t flow_pub = -1;
@ -191,8 +194,10 @@ static struct mavlink_subscriptions {
static struct mavlink_publications {
orb_advert_t offboard_control_sp_pub;
orb_advert_t vicon_position_pub;
} mavlink_pubs = {
.offboard_control_sp_pub = -1
.offboard_control_sp_pub = -1,
.vicon_position_pub = -1
};
@ -1240,10 +1245,27 @@ void handleMessage(mavlink_message_t *msg)
/* check if topic is advertised */
if (cmd_pub <= 0) {
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
}
} else {
/* create command */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
}
}
/* Handle Vicon position estimates */
if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
mavlink_vicon_position_estimate_t pos;
mavlink_msg_vicon_position_estimate_decode(msg, &pos);
vicon_position.x = pos.x;
vicon_position.y = pos.y;
vicon_position.z = pos.z;
if (mavlink_pubs.vicon_position_pub <= 0) {
mavlink_pubs.vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
} else {
orb_publish(ORB_ID(vehicle_vicon_position), mavlink_pubs.vicon_position_pub, &vicon_position);
}
}
/* Handle quadrotor motor setpoints */

34
apps/multirotor_pos_control/multirotor_pos_control.c
View File

@ -55,9 +55,11 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <systemlib/systemlib.h>
#include "multirotor_pos_control_params.h"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
@ -144,7 +146,7 @@ multirotor_pos_control_thread_main(int argc, char *argv[])
struct vehicle_attitude_s att;
//struct vehicle_global_position_setpoint_s global_pos_sp;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_local_position_s local_pos;
struct vehicle_vicon_position_s local_pos;
struct manual_control_setpoint_s manual;
struct vehicle_attitude_setpoint_s att_sp;
@ -152,7 +154,7 @@ multirotor_pos_control_thread_main(int argc, char *argv[])
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
//int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
@ -161,6 +163,14 @@ multirotor_pos_control_thread_main(int argc, char *argv[])
thread_running = true;
int loopcounter = 0;
struct multirotor_position_control_params p;
struct multirotor_position_control_param_handles h;
parameters_init(&h);
parameters_update(&h, &p);
while (1) {
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
@ -169,16 +179,27 @@ multirotor_pos_control_thread_main(int argc, char *argv[])
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
/* get a local copy of local position */
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos);
/* get a local copy of local position setpoint */
orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
if (loopcounter == 500) {
parameters_update(&h, &p);
loopcounter = 0;
}
// if (state.state_machine == SYSTEM_STATE_AUTO) {
// XXX IMPLEMENT POSITION CONTROL HERE
att_sp.roll_body = 0.1f;
att_sp.pitch_body = 0.0f;
float dT = 1.0f / 50.0f;
float x_setpoint = 0.0f;
/* local pos is the Vicon position */
att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p * dT;
att_sp.roll_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.4f;
att_sp.timestamp = hrt_absolute_time();
@ -199,6 +220,7 @@ multirotor_pos_control_thread_main(int argc, char *argv[])
/* run at approximately 50 Hz */
usleep(20000);
loopcounter++;
}

62
apps/multirotor_pos_control/multirotor_pos_control_params.c Normal file
View File

@ -0,0 +1,62 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* 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 multirotor_position_control_params.c
*
* Parameters for EKF filter
*/
#include "multirotor_pos_control_params.h"
/* Extended Kalman Filter covariances */
/* controller parameters */
PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f);
int parameters_init(struct multirotor_position_control_param_handles *h)
{
/* PID parameters */
h->p = param_find("MC_POS_P");
return OK;
}
int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
{
param_get(h->p, &(p->p));
return OK;
}

66
apps/multirotor_pos_control/multirotor_pos_control_params.h Normal file
View File

@ -0,0 +1,66 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* 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 multirotor_position_control_params.h
*
* Parameters for position controller
*/
#include <systemlib/param/param.h>
struct multirotor_position_control_params {
float p;
float i;
float d;
};
struct multirotor_position_control_param_handles {
param_t p;
param_t i;
param_t d;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct multirotor_position_control_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p);

3
apps/uORB/objects_common.cpp
View File

@ -77,6 +77,9 @@ ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
#include "topics/vehicle_local_position.h"
ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s);
#include "topics/vehicle_vicon_position.h"
ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
#include "topics/vehicle_rates_setpoint.h"
ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);

78
apps/uORB/topics/vehicle_vicon_position.h Normal file
View File

@ -0,0 +1,78 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
*
* 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 vehicle_vicon_position.h
* Definition of the raw VICON Motion Capture position
*/
#ifndef TOPIC_VEHICLE_VICON_POSITION_H_
#define TOPIC_VEHICLE_VICON_POSITION_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/**
* Fused local position in NED.
*/
struct vehicle_vicon_position_s
{
uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
bool valid; /**< true if position satisfies validity criteria of estimator */
float x; /**< X positin in meters in NED earth-fixed frame */
float y; /**< X positin in meters in NED earth-fixed frame */
float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */
float vx;
float vy;
float vz;
// TODO Add covariances here
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_vicon_position);
#endif