Browse Source

Topics: Move from global_position_setpoint to mission_item_triplet

sbg
Julian Oes 11 years ago
parent
commit
1c7e07d8d7
  1. 1
      src/examples/flow_position_control/flow_position_control_main.c
  2. 1
      src/modules/controllib/uorb/blocks.hpp
  3. 27
      src/modules/mavlink/orb_listener.c
  4. 4
      src/modules/mavlink/orb_topics.h
  5. 2
      src/modules/mavlink_onboard/orb_topics.h
  6. 39
      src/modules/multirotor_pos_control/multirotor_pos_control.c
  7. 37
      src/modules/sdlog2/sdlog2.c
  8. 11
      src/modules/sdlog2/sdlog2_messages.h
  9. 3
      src/modules/uORB/objects_common.cpp
  10. 86
      src/modules/uORB/topics/vehicle_global_position_setpoint.h

1
src/examples/flow_position_control/flow_position_control_main.c

@ -61,7 +61,6 @@ @@ -61,7 +61,6 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/filtered_bottom_flow.h>
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>

1
src/modules/controllib/uorb/blocks.hpp

@ -43,7 +43,6 @@ @@ -43,7 +43,6 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>

27
src/modules/mavlink/orb_listener.c

@ -136,7 +136,7 @@ static const struct listener listeners[] = { @@ -136,7 +136,7 @@ static const struct listener listeners[] = {
{l_input_rc, &mavlink_subs.input_rc_sub, 0},
{l_global_position, &mavlink_subs.global_pos_sub, 0},
{l_local_position, &mavlink_subs.local_pos_sub, 0},
{l_global_position_setpoint, &mavlink_subs.spg_sub, 0},
{l_global_position_setpoint, &mavlink_subs.triplet_sub, 0},
{l_local_position_setpoint, &mavlink_subs.spl_sub, 0},
{l_attitude_setpoint, &mavlink_subs.spa_sub, 0},
{l_actuator_outputs, &mavlink_subs.act_0_sub, 0},
@ -402,23 +402,24 @@ l_local_position(const struct listener *l) @@ -402,23 +402,24 @@ l_local_position(const struct listener *l)
void
l_global_position_setpoint(const struct listener *l)
{
struct vehicle_global_position_setpoint_s global_sp;
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp);
struct mission_item_triplet_s triplet;
orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet);
uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
if (!triplet.current_valid)
return;
if (global_sp.altitude_is_relative)
if (triplet.current.altitude_is_relative)
coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
if (gcs_link)
mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
coordinate_frame,
global_sp.lat,
global_sp.lon,
global_sp.altitude * 1000.0f,
global_sp.yaw * M_RAD_TO_DEG_F * 100.0f);
(int32_t)(triplet.current.lat * 1e7f),
(int32_t)(triplet.current.lon * 1e7f),
(int32_t)(triplet.current.altitude * 1e3f),
(int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f));
}
void
@ -770,9 +771,9 @@ uorb_receive_start(void) @@ -770,9 +771,9 @@ uorb_receive_start(void)
orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */
/* --- GLOBAL SETPOINT VALUE --- */
mavlink_subs.spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
orb_set_interval(mavlink_subs.spg_sub, 2000); /* 0.5 Hz updates */
mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */
/* --- LOCAL SETPOINT VALUE --- */
mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */

4
src/modules/mavlink/orb_topics.h

@ -50,8 +50,8 @@ @@ -50,8 +50,8 @@
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@ -86,7 +86,7 @@ struct mavlink_subscriptions { @@ -86,7 +86,7 @@ struct mavlink_subscriptions {
int local_pos_sub;
int spa_sub;
int spl_sub;
int spg_sub;
int triplet_sub;
int debug_key_value;
int input_rc_sub;
int optical_flow;

2
src/modules/mavlink_onboard/orb_topics.h

@ -50,7 +50,7 @@ @@ -50,7 +50,7 @@
#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/mission_item_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/optical_flow.h>

39
src/modules/multirotor_pos_control/multirotor_pos_control.c

@ -61,8 +61,8 @@ @@ -61,8 +61,8 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/mission_item_triplet.h>
#include <systemlib/systemlib.h>
#include <systemlib/pid/pid.h>
#include <mavlink/mavlink_log.h>
@ -190,12 +190,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) @@ -190,12 +190,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
memset(&manual, 0, sizeof(manual));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct vehicle_local_position_setpoint_s local_pos_sp;
memset(&local_pos_sp, 0, sizeof(local_pos_sp));
struct vehicle_global_position_setpoint_s global_pos_sp;
memset(&global_pos_sp, 0, sizeof(global_pos_sp));
struct mission_item_triplet_s triplet;
memset(&triplet, 0, sizeof(triplet));
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(global_vel_sp));
struct vehicle_local_position_setpoint_s local_pos_sp;
memset(&local_pos_sp, 0, sizeof(local_pos_sp));
/* subscribe to attitude, motor setpoints and system state */
int param_sub = orb_subscribe(ORB_ID(parameter_update));
@ -203,9 +203,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) @@ -203,9 +203,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int mission_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
/* publish setpoint */
orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
@ -292,11 +290,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) @@ -292,11 +290,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
}
orb_check(global_pos_sp_sub, &updated);
orb_check(mission_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
global_pos_sp_valid = true;
orb_copy(ORB_ID(mission_item_triplet), mission_triplet_sub, &triplet);
global_pos_sp_valid = triplet.current_valid;
reset_mission_sp = true;
}
@ -329,7 +327,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) @@ -329,7 +327,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
@ -459,24 +456,22 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) @@ -459,24 +456,22 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
/* update global setpoint projection */
if (global_pos_sp_valid) {
/* global position setpoint valid, use it */
double sp_lat = global_pos_sp.lat * 1e-7;
double sp_lon = global_pos_sp.lon * 1e-7;
/* project global setpoint to local setpoint */
map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
map_projection_project(triplet.current.lat, triplet.current.lon, &(local_pos_sp.x), &(local_pos_sp.y));
if (global_pos_sp.altitude_is_relative) {
local_pos_sp.z = -global_pos_sp.altitude;
if (triplet.current.altitude_is_relative) {
local_pos_sp.z = -triplet.current.altitude;
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
local_pos_sp.z = local_pos.ref_alt - triplet.current.lat;
}
/* update yaw setpoint only if value is valid */
if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
att_sp.yaw_body = global_pos_sp.yaw;
if (isfinite(triplet.current.yaw) && fabsf(triplet.current.yaw) < M_TWOPI) {
att_sp.yaw_body = triplet.current.yaw;
}
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", triplet.current.lat, triplet.current.lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
} else {
if (reset_auto_sp_xy) {

37
src/modules/sdlog2/sdlog2.c

@ -72,7 +72,7 @@ @@ -72,7 +72,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
@ -693,7 +693,7 @@ int sdlog2_thread_main(int argc, char *argv[]) @@ -693,7 +693,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
struct vehicle_global_position_setpoint_s global_pos_sp;
struct mission_item_triplet_s triplet;
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
struct optical_flow_s flow;
@ -718,7 +718,7 @@ int sdlog2_thread_main(int argc, char *argv[]) @@ -718,7 +718,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int local_pos_sub;
int local_pos_sp_sub;
int global_pos_sub;
int global_pos_sp_sub;
int triplet_sub;
int gps_pos_sub;
int vicon_pos_sub;
int flow_sub;
@ -840,8 +840,8 @@ int sdlog2_thread_main(int argc, char *argv[]) @@ -840,8 +840,8 @@ int sdlog2_thread_main(int argc, char *argv[])
fdsc_count++;
/* --- GLOBAL POSITION SETPOINT--- */
subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
fds[fdsc_count].fd = subs.global_pos_sp_sub;
subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet));
fds[fdsc_count].fd = subs.triplet_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@ -1150,20 +1150,21 @@ int sdlog2_thread_main(int argc, char *argv[]) @@ -1150,20 +1150,21 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GLOBAL POSITION SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp);
orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet);
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative;
log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat;
log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon;
log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude;
log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw;
log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius;
log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction;
log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd;
log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1;
log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2;
log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3;
log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4;
log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative;
log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7);
log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7);
log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude;
log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd;
log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
log_msg.body.log_GPSP.radius = buf.triplet.current.radius;
log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside;
log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
LOGBUFFER_WRITE_AND_COUNT(GPSP);
}

11
src/modules/sdlog2/sdlog2_messages.h

@ -214,13 +214,12 @@ struct log_GPSP_s { @@ -214,13 +214,12 @@ struct log_GPSP_s {
int32_t lon;
float altitude;
float yaw;
uint8_t nav_cmd;
float loiter_radius;
int8_t loiter_direction;
uint8_t nav_cmd;
float param1;
float param2;
float param3;
float param4;
float radius;
float time_inside;
float pitch_min;
};
/* --- ESC - ESC STATE --- */
@ -288,7 +287,7 @@ static const struct log_format_s log_formats[] = { @@ -288,7 +287,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitRad,LoitDir,Rad,TimeIn,pitMin"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
/* system-level messages, ID >= 0x80 */

3
src/modules/uORB/objects_common.cpp

@ -117,9 +117,6 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi @@ -117,9 +117,6 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi
#include "topics/vehicle_bodyframe_speed_setpoint.h"
ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s);
#include "topics/vehicle_global_position_setpoint.h"
ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s);
#include "topics/mission_item_triplet.h"
ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s);

86
src/modules/uORB/topics/vehicle_global_position_setpoint.h

@ -1,86 +0,0 @@ @@ -1,86 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @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_global_position_setpoint.h
* Definition of the global WGS84 position setpoint uORB topic.
*/
#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_
#define TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
#include "mission.h"
/**
* @addtogroup topics
* @{
*/
/**
* Global position setpoint in WGS84 coordinates.
*
* This is the position the MAV is heading towards. If it of type loiter,
* the MAV is circling around it with the given loiter radius in meters.
*/
struct vehicle_global_position_setpoint_s
{
bool altitude_is_relative; /**< true if altitude is relative from start point */
int32_t lat; /**< latitude in degrees * 1E7 */
int32_t lon; /**< longitude in degrees * 1E7 */
float altitude; /**< altitude in meters */
float yaw; /**< in radians NED -PI..+PI */
float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */
int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */
enum NAV_CMD nav_cmd; /**< true if loitering is enabled */
float param1;
float param2;
float param3;
float param4;
float turn_distance_xy; /**< The distance on the plane which will mark this as reached */
float turn_distance_z; /**< The distance in Z direction which will mark this as reached */
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(vehicle_global_position_setpoint);
#endif
Loading…
Cancel
Save