yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Reworked control interface, needs testing / validation

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-19 18:53:29 +02:00
parent b0b72b11eb
commit 291f4f3a33
12 changed files with 165 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
apps/commander/commander.c
View File

@@ -742,6 +742,31 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
}
break;
case PX4_CMD_CONTROLLER_SELECTION: {
bool changed = false;
if ((int)cmd->param1 != (int)current_vehicle_status->flag_control_rates_enabled) {
current_vehicle_status->flag_control_rates_enabled = cmd->param1;
changed = true;
}
if ((int)cmd->param2 != (int)current_vehicle_status->flag_control_attitude_enabled) {
current_vehicle_status->flag_control_attitude_enabled = cmd->param2;
changed = true;
}
if ((int)cmd->param3 != (int)current_vehicle_status->flag_control_velocity_enabled) {
current_vehicle_status->flag_control_velocity_enabled = cmd->param3;
changed = true;
}
if ((int)cmd->param4 != (int)current_vehicle_status->flag_control_position_enabled) {
current_vehicle_status->flag_control_position_enabled = cmd->param4;
changed = true;
}
if (changed) {
/* publish current state machine */
state_machine_publish(status_pub, current_vehicle_status, mavlink_fd);
}
}
// /* request to land */
// case MAV_CMD_NAV_LAND:
// {

103
apps/mavlink/mavlink.c
View File

@@ -65,7 +65,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/ardrone_motors_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
@@ -126,14 +126,13 @@ static struct vehicle_attitude_s hil_attitude;
static struct vehicle_global_position_s hil_global_pos;
static struct ardrone_motors_setpoint_s ardrone_motors;
static struct vehicle_rates_setpoint_s vehicle_rates_sp;
static struct vehicle_command_s vcmd;
static struct actuator_armed_s armed;
static orb_advert_t pub_hil_global_pos = -1;
static orb_advert_t ardrone_motors_pub = -1;
static orb_advert_t cmd_pub = -1;
static orb_advert_t flow_pub = -1;
static orb_advert_t global_position_setpoint_pub = -1;
@@ -188,6 +187,12 @@ static struct mavlink_subscriptions {
.initialized = false
};
static struct mavlink_publications {
orb_advert_t vehicle_rates_sp_pub;
} mavlink_pubs = {
.vehicle_rates_sp_pub = -1
};
/* 3: Define waypoint helper functions */
void mavlink_wpm_send_message(mavlink_message_t *msg);
@@ -1133,6 +1138,13 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
}
}
#define MAVLINK_OFFBOARD_CONTROL_MODE_NONE 0
#define MAVLINK_OFFBOARD_CONTROL_MODE_RATES 1
#define MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE 2
#define MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY 3
#define MAVLINK_OFFBOARD_CONTROL_MODE_POSITION 4
#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10
/****************************************************************************
* Public Functions
****************************************************************************/
@@ -1242,29 +1254,80 @@ void handleMessage(mavlink_message_t *msg)
// printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", quad_motors_setpoint.target_system, mavlink_system.sysid);
if (mavlink_system.sysid < 4) {
ardrone_motors.p1 = quad_motors_setpoint.roll[mavlink_system.sysid];
ardrone_motors.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid];
ardrone_motors.p3 = quad_motors_setpoint.yaw[mavlink_system.sysid];
ardrone_motors.p4 = quad_motors_setpoint.thrust[mavlink_system.sysid];
ardrone_motors.timestamp = hrt_absolute_time();
/* only send if RC is off */
if (v_status.rc_signal_lost) {
/* check if input has to be enabled */
if (!v_status.flag_control_offboard_enabled) {
/* XXX Enable offboard control */
}
/* XXX decode mode and set flags */
// if (mode == abc) xxx flag_control_rates_enabled;
/* rate control mode */
if (quad_motors_setpoint.mode == 1) {
vehicle_rates_sp.roll = quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX;
vehicle_rates_sp.pitch = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX;
vehicle_rates_sp.yaw = quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX;
vehicle_rates_sp.thrust = quad_motors_setpoint.thrust[mavlink_system.sysid] / (float)UINT16_MAX;
vehicle_rates_sp.timestamp = hrt_absolute_time();
}
/* check if topic has to be advertised */
if (ardrone_motors_pub <= 0) {
ardrone_motors_pub = orb_advertise(ORB_ID(ardrone_motors_setpoint), &ardrone_motors);
if (mavlink_pubs.vehicle_rates_sp_pub <= 0) {
mavlink_pubs.vehicle_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &vehicle_rates_sp);
}
/* Publish */
orb_publish(ORB_ID(ardrone_motors_setpoint), ardrone_motors_pub, &ardrone_motors);
orb_publish(ORB_ID(vehicle_rates_setpoint), mavlink_pubs.vehicle_rates_sp_pub, &vehicle_rates_sp);
/* change armed status if required */
bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED);
bool cmd_generated = false;
if (v_status.flag_control_offboard_enabled != cmd_armed) {
vcmd.param1 = cmd_armed;
vcmd.param2 = 0;
vcmd.param3 = 0;
vcmd.param4 = 0;
vcmd.param5 = 0;
vcmd.param6 = 0;
vcmd.param7 = 0;
vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM;
vcmd.target_system = mavlink_system.sysid;
vcmd.target_component = MAV_COMP_ID_ALL;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
vcmd.confirmation = 1;
cmd_generated = true;
}
/* check if input has to be enabled */
if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) ||
(v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) ||
(v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) ||
(v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) {
vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES);
vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE);
vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY);
vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION);
vcmd.param5 = 0;
vcmd.param6 = 0;
vcmd.param7 = 0;
vcmd.command = PX4_CMD_CONTROLLER_SELECTION;
vcmd.target_system = mavlink_system.sysid;
vcmd.target_component = MAV_COMP_ID_ALL;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
vcmd.confirmation = 1;
cmd_generated = true;
}
if (cmd_generated) {
/* 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);
}
}
}
}
}
@@ -1479,7 +1542,7 @@ int mavlink_thread_main(int argc, char *argv[])
memset(&rc, 0, sizeof(rc));
memset(&hil_attitude, 0, sizeof(hil_attitude));
memset(&hil_global_pos, 0, sizeof(hil_global_pos));
memset(&ardrone_motors, 0, sizeof(ardrone_motors));
memset(&vehicle_rates_sp, 0, sizeof(vehicle_rates_sp));
memset(&vcmd, 0, sizeof(vcmd));
/* print welcome text */

31
apps/multirotor_att_control/multirotor_att_control_main.c
View File

@@ -58,13 +58,14 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/ardrone_motors_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/perf_counter.h>
#include "multirotor_attitude_control.h"
#include "multirotor_rate_control.h"
__EXPORT int multirotor_att_control_main(int argc, char *argv[]);
@@ -93,18 +94,18 @@ mc_thread_main(int argc, char *argv[])
memset(&manual, 0, sizeof(manual));
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
struct ardrone_motors_setpoint_s setpoint;
memset(&setpoint, 0, sizeof(setpoint));
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
struct actuator_controls_s actuators;
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
/*
* Do not rate-limit the loop to prevent aliasing
@@ -145,6 +146,12 @@ mc_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
/* get a local copy of attitude setpoint */
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
/* get a local copy of rates setpoint */
bool updated;
orb_check(setpoint_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_rates_setpoint), setpoint_sub, &rates_sp);
}
/* get a local copy of the current sensor values */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
@@ -155,16 +162,15 @@ mc_thread_main(int argc, char *argv[])
/* manual inputs, from RC control or joystick */
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
att_sp.yaw_rate_body = manual.yaw;
att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller
/* set yaw rate */
rates_sp.yaw = manual.yaw;
att_sp.timestamp = hrt_absolute_time();
if (motor_test_mode) {
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.roll_rate_body = 0.0f;
att_sp.pitch_rate_body = 0.0f;
att_sp.yaw_rate_body = 0.0f;
att_sp.thrust = 0.1f;
} else {
att_sp.thrust = manual.throttle;
@@ -184,7 +190,14 @@ mc_thread_main(int argc, char *argv[])
/* XXX could be also run in an independent loop */
if (state.flag_control_rates_enabled) {
multirotor_control_rates(&att_sp, &raw.gyro_rad_s, &actuators);
/* lowpass gyros */
// XXX
static float gyro_lp[3] = {0, 0, 0};
gyro_lp[0] = raw.gyro_rad_s[0];
gyro_lp[1] = raw.gyro_rad_s[1];
gyro_lp[2] = raw.gyro_rad_s[2];
multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
}
/* STEP 3: publish the result to the vehicle actuators */

2
apps/multirotor_att_control/multirotor_attitude_control.c
View File

@@ -243,7 +243,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
att->roll, att->rollspeed, deltaT);
/* control yaw rate */
float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_rate_body, att->yawspeed, 0.0f, deltaT);
float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT);
/*
* compensate the vertical loss of thrust

8
apps/multirotor_att_control/multirotor_rate_control.c
View File

@@ -132,7 +132,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
return OK;
}
void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators)
{
static uint64_t last_run = 0;
@@ -179,13 +179,13 @@ void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
/* calculate current control outputs */
/* control pitch (forward) output */
float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch_rate_body,
float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch,
rates[1], 0.0f, deltaT);
/* control roll (left/right) output */
float roll_control = pid_calculate(&roll_controller, rate_sp->roll_rate_body,
float roll_control = pid_calculate(&roll_controller, rate_sp->roll,
rates[0], 0.0f, deltaT);
/* control yaw rate */
float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw_rate_body, rates[2], 0.0f, deltaT);
float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
/*
* compensate the vertical loss of thrust

4
apps/multirotor_att_control/multirotor_rate_control.h
View File

@@ -47,10 +47,10 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators);
#endif /* MULTIROTOR_RATE_CONTROL_H_ */

2
apps/px4/ground_estimator/ground_estimator.c
View File

@@ -75,7 +75,7 @@ int ground_estimator_thread_main(int argc, char *argv[]) {
/* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* advertise attitude */
/* advertise debug value */
struct debug_key_value_s dbg = { .key = "posx", .value = 0.0f };
orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);

4
apps/uORB/objects_common.cpp
View File

@@ -78,8 +78,8 @@ 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/ardrone_motors_setpoint.h"
ORB_DEFINE(ardrone_motors_setpoint, struct ardrone_motors_setpoint_s);
#include "topics/vehicle_rates_setpoint.h"
ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
#include "topics/rc_channels.h"
ORB_DEFINE(rc_channels, struct rc_channels_s);

17
apps/uORB/topics/vehicle_attitude_setpoint.h
View File

@@ -56,21 +56,18 @@ struct vehicle_attitude_setpoint_s
{
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */
float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */
float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */
float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */
//float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */
//float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */
//float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */
//float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */
float roll_body; /**< body angle in NED frame */
float pitch_body; /**< body angle in NED frame */
float yaw_body; /**< body angle in NED frame */
float roll_rate_body; /**< body angle in NED frame */
float pitch_rate_body; /**< body angle in NED frame */
float yaw_rate_body; /**< body angle in NED frame */
float body_valid; /**< Set to true if Tait-Bryan angles are valid */
//float body_valid; /**< Set to true if body angles are valid */
float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */
bool R_valid; /**< Set to true if rotation matrix is valid */
//float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */
//bool R_valid; /**< Set to true if rotation matrix is valid */
float thrust; /**< Thrust in Newton the power system should generate */

4
apps/uORB/topics/vehicle_command.h
View File

@@ -50,6 +50,10 @@
* @{
*/
enum PX4_CMD {
PX4_CMD_CONTROLLER_SELECTION = 1000,
};
struct vehicle_command_s
{
float param1; /**< Parameter 1, as defined by MAVLink MAV_CMD enum. */

27
apps/uORB/topics/ardrone_motors_setpoint.h → apps/uORB/topics/vehicle_rates_setpoint.h
View File

@@ -33,12 +33,12 @@
****************************************************************************/
/**
* @file ardrone_motors_setpoint.h
* Definition of the ardrone_motors_setpoint uORB topic.
* @file vehicle_rates_setpoint.h
* Definition of the vehicle rates setpoint topic
*/
#ifndef TOPIC_ARDRONE_MOTORS_SETPOINT_H_
#define TOPIC_ARDRONE_MOTORS_SETPOINT_H_
#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_
#define TOPIC_VEHICLE_RATES_SETPOINT_H_
#include <stdint.h>
#include "../uORB.h"
@@ -48,23 +48,22 @@
* @{
*/
struct ardrone_motors_setpoint_s
struct vehicle_rates_setpoint_s
{
uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data
uint64_t timestamp; /**< in microseconds since system start */
uint8_t group; /**< quadrotor group */
uint8_t mode; /**< requested flight mode XXX define */
float p1; /**< parameter 1: rate control: roll rate, att control: roll angle (in radians, NED) */
float p2; /**< parameter 2: rate control: pitch rate, att control: pitch angle (in radians, NED) */
float p3; /**< parameter 3: rate control: yaw rate, att control: yaw angle (in radians, NED) */
float p4; /**< parameter 4: thrust, [0..1] */
}; /**< AR.Drone low level motors */
float roll; /**< body angular rates in NED frame */
float pitch; /**< body angular rates in NED frame */
float yaw; /**< body angular rates in NED frame */
float thrust; /**< thrust normalized to 0..1 */
}; /**< vehicle_rates_setpoint */
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(ardrone_motors_setpoint);
ORB_DECLARE(vehicle_rates_setpoint);
#endif

2
apps/uORB/topics/vehicle_status.h
View File

@@ -127,7 +127,7 @@ struct vehicle_status_s
bool flag_control_rates_enabled; /**< true if rates are stabilized */
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
bool flag_control_speed_enabled; /**< true if speed (implies direction) is controlled */
bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */
bool flag_control_position_enabled; /**< true if position is controlled */
bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */