fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'inav_fix' into inav_flow

Browse Source
This commit is contained in:
Anton Babushkin 2013-10-07 12:00:24 +02:00
commit bda03cadc3
7 changed files with 234 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
ROMFS/px4fmu_common/init.d/rc.custom_io_esc Normal file
View File

@ -0,0 +1,120 @@
#!nsh
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.002
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.09
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 6.8
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWRATE_D 0.005
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_P 0.3
param set NAV_TAKEOFF_ALT 3.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.7
param set MPC_THR_MIN 0.3
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
param save
fi
echo "RC script for PX4FMU + PX4IO + PPM-ESCs running"
#
# Force some key parameters to sane values
# MAV_TYPE 2 = quadrotor
#
param set MAV_TYPE 2
set EXIT_ON_END no
usleep 10000
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start -d /dev/ttyS1 -b 115200
usleep 5000
sh /etc/init.d/rc.io
if [ $ESC_MAKER = afro ]
then
# Set PWM values for Afro ESCs
px4io idle 1050 1050 1050 1050
px4io min 1080 1080 1080 1080
px4io max 1860 1860 1860 1860
else
# Set PWM values for typical ESCs
px4io idle 900 900 900 900
px4io min 1110 1100 1100 1100
px4io max 1800 1800 1800 1800
fi
else
fmu mode_pwm
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS1 -b 115200
usleep 5000
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer
#
if [ $FRAME_GEOMETRY == x ]
then
echo "Frame geometry X"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
else
if [ $FRAME_GEOMETRY == w ]
then
echo "Frame geometry W"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
else
echo "Frame geometry +"
mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
fi
fi
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi
echo "Script end"

17
ROMFS/px4fmu_common/init.d/rcS
View File

@ -225,6 +225,23 @@ then
sh /etc/init.d/rc.custom_dji_f330_mkblctrl
set MODE custom
fi
# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
if param compare SYS_AUTOSTART 21
then
set FRAME_GEOMETRY x
set ESC_MAKER afro
sh /etc/init.d/rc.custom_io_esc
set MODE custom
fi
# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
if param compare SYS_AUTOSTART 22
then
set FRAME_GEOMETRY w
sh /etc/init.d/rc.custom_io_esc
set MODE custom
fi
if param compare SYS_AUTOSTART 30
then

12
src/drivers/px4io/px4io.cpp
View File

@ -94,6 +94,8 @@ extern device::Device *PX4IO_serial_interface() weak_function;
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
#define UPDATE_INTERVAL_MIN 2
/**
* The PX4IO class.
*
@ -790,8 +792,8 @@ PX4IO::task_main()
/* adjust update interval */
if (_update_interval != 0) {
if (_update_interval < 5)
_update_interval = 5;
if (_update_interval < UPDATE_INTERVAL_MIN)
_update_interval = UPDATE_INTERVAL_MIN;
if (_update_interval > 100)
_update_interval = 100;
orb_set_interval(_t_actuators, _update_interval);
@ -1942,8 +1944,8 @@ int
PX4IO::set_update_rate(int rate)
{
int interval_ms = 1000 / rate;
if (interval_ms < 3) {
interval_ms = 3;
if (interval_ms < UPDATE_INTERVAL_MIN) {
interval_ms = UPDATE_INTERVAL_MIN;
warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
}
@ -2317,7 +2319,7 @@ px4io_main(int argc, char *argv[])
if ((argc > 2)) {
g_dev->set_update_rate(atoi(argv[2]));
} else {
errx(1, "missing argument (50 - 400 Hz)");
errx(1, "missing argument (50 - 500 Hz)");
return 1;
}
exit(0);

36
src/examples/flow_position_control/flow_position_control_main.c
View File

@ -67,6 +67,7 @@
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <poll.h>
#include <mavlink/mavlink_log.h>
#include "flow_position_control_params.h"
@ -153,20 +154,28 @@ flow_position_control_thread_main(int argc, char *argv[])
{
/* welcome user */
thread_running = true;
printf("[flow position control] starting\n");
static int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[fpc] started");
uint32_t counter = 0;
const float time_scale = powf(10.0f,-6.0f);
/* structures */
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
struct filtered_bottom_flow_s filtered_flow;
memset(&filtered_flow, 0, sizeof(filtered_flow));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct vehicle_bodyframe_speed_setpoint_s speed_sp;
memset(&speed_sp, 0, sizeof(speed_sp));
/* subscribe to attitude, motor setpoints and system state */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
@ -216,6 +225,7 @@ flow_position_control_thread_main(int argc, char *argv[])
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err");
static bool sensors_ready = false;
static bool status_changed = false;
while (!thread_should_exit)
{
@ -252,7 +262,7 @@ flow_position_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
parameters_update(&param_handles, &params);
printf("[flow position control] parameters updated.\n");
mavlink_log_info(mavlink_fd,"[fpc] parameters updated.");
}
/* only run controller if position/speed changed */
@ -270,6 +280,8 @@ flow_position_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
/* get a local copy of local position */
orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
/* get a local copy of control mode */
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
if (control_mode.flag_control_velocity_enabled)
{
@ -277,6 +289,11 @@ flow_position_control_thread_main(int argc, char *argv[])
float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1
if(status_changed == false)
mavlink_log_info(mavlink_fd,"[fpc] flow POSITION control engaged");
status_changed = true;
/* calc dt */
if(last_time == 0)
{
@ -296,7 +313,7 @@ flow_position_control_thread_main(int argc, char *argv[])
{
flow_sp_sumy = filtered_flow.sumy;
update_flow_sp_sumy = false;
}
}
/* calc new bodyframe speed setpoints */
float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d;
@ -518,6 +535,11 @@ flow_position_control_thread_main(int argc, char *argv[])
else
{
/* in manual or stabilized state just reset speed and flow sum setpoint */
//mavlink_log_info(mavlink_fd,"[fpc] reset speed sp, flow_sp_sumx,y (%f,%f)",filtered_flow.sumx, filtered_flow.sumy);
if(status_changed == true)
mavlink_log_info(mavlink_fd,"[fpc] flow POSITION controller disengaged.");
status_changed = false;
speed_sp.vx = 0.0f;
speed_sp.vy = 0.0f;
flow_sp_sumx = filtered_flow.sumx;
@ -558,20 +580,20 @@ flow_position_control_thread_main(int argc, char *argv[])
else if (ret == 0)
{
/* no return value, ignore */
printf("[flow position control] no attitude received.\n");
mavlink_log_info(mavlink_fd,"[fpc] no attitude received.\n");
}
else
{
if (fds[0].revents & POLLIN)
{
sensors_ready = true;
printf("[flow position control] initialized.\n");
mavlink_log_info(mavlink_fd,"[fpc] initialized.\n");
}
}
}
}
printf("[flow position control] ending now...\n");
mavlink_log_info(mavlink_fd,"[fpc] ending now...\n");
thread_running = false;

7
src/examples/flow_position_estimator/flow_position_estimator_main.c
View File

@ -345,6 +345,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
local_pos.y = local_pos.y + global_speed[1] * dt;
local_pos.vx = global_speed[0];
local_pos.vy = global_speed[1];
local_pos.xy_valid = true;
local_pos.v_xy_valid = true;
}
else
{
@ -353,6 +355,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
filtered_flow.vy = 0;
local_pos.vx = 0;
local_pos.vy = 0;
local_pos.xy_valid = false;
local_pos.v_xy_valid = false;
}
/* filtering ground distance */
@ -361,6 +365,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
/* not possible to fly */
sonar_valid = false;
local_pos.z = 0.0f;
local_pos.z_valid = false;
}
else
{
@ -388,6 +393,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
{
local_pos.z = -sonar_new;
}
local_pos.z_valid = true;
}
filtered_flow.timestamp = hrt_absolute_time();

32
src/examples/flow_speed_control/flow_speed_control_main.c
View File

@ -65,6 +65,7 @@
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <poll.h>
#include <mavlink/mavlink_log.h>
#include "flow_speed_control_params.h"
@ -151,17 +152,23 @@ flow_speed_control_thread_main(int argc, char *argv[])
{
/* welcome user */
thread_running = true;
printf("[flow speed control] starting\n");
static int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd,"[fsc] started");
uint32_t counter = 0;
/* structures */
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct filtered_bottom_flow_s filtered_flow;
memset(&filtered_flow, 0, sizeof(filtered_flow));
struct vehicle_bodyframe_speed_setpoint_s speed_sp;
memset(&speed_sp, 0, sizeof(speed_sp));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
/* subscribe to attitude, motor setpoints and system state */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
@ -186,6 +193,7 @@ flow_speed_control_thread_main(int argc, char *argv[])
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err");
static bool sensors_ready = false;
static bool status_changed = false;
while (!thread_should_exit)
{
@ -221,7 +229,7 @@ flow_speed_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
parameters_update(&param_handles, &params);
printf("[flow speed control] parameters updated.\n");
mavlink_log_info(mavlink_fd,"[fsp] parameters updated.");
}
/* only run controller if position/speed changed */
@ -237,6 +245,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
/* get a local copy of bodyframe speed setpoint */
orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp);
/* get a local copy of control mode */
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
if (control_mode.flag_control_velocity_enabled)
{
@ -244,6 +254,11 @@ flow_speed_control_thread_main(int argc, char *argv[])
float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p;
float roll_body = (speed_sp.vy - filtered_flow.vy) * params.speed_p;
if(status_changed == false)
mavlink_log_info(mavlink_fd,"[fsc] flow SPEED control engaged");
status_changed = true;
/* limit roll and pitch corrections */
if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch))
{
@ -299,6 +314,11 @@ flow_speed_control_thread_main(int argc, char *argv[])
}
else
{
if(status_changed == true)
mavlink_log_info(mavlink_fd,"[fsc] flow SPEED controller disengaged.");
status_changed = false;
/* reset attitude setpoint */
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
@ -334,20 +354,20 @@ flow_speed_control_thread_main(int argc, char *argv[])
else if (ret == 0)
{
/* no return value, ignore */
printf("[flow speed control] no attitude received.\n");
mavlink_log_info(mavlink_fd,"[fsc] no attitude received.");
}
else
{
if (fds[0].revents & POLLIN)
{
sensors_ready = true;
printf("[flow speed control] initialized.\n");
mavlink_log_info(mavlink_fd,"[fsp] initialized.");
}
}
}
}
printf("[flow speed control] ending now...\n");
mavlink_log_info(mavlink_fd,"[fsc] ending now...");
thread_running = false;

54
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -81,7 +81,7 @@ static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms
static const hrt_abstime sonar_valid_timeout = 1000000; // assume that altitude == distance to surface during this time
static const hrt_abstime flow_timeout = 1000000; // optical flow topic timeout = 1s
static const uint32_t updates_counter_len = 1000000;
static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz
static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz
__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
@ -315,14 +315,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* main loop */
struct pollfd fds[7] = {
{ .fd = parameter_update_sub, .events = POLLIN },
{ .fd = actuator_sub, .events = POLLIN },
{ .fd = armed_sub, .events = POLLIN },
struct pollfd fds[1] = {
{ .fd = vehicle_attitude_sub, .events = POLLIN },
{ .fd = sensor_combined_sub, .events = POLLIN },
{ .fd = optical_flow_sub, .events = POLLIN },
{ .fd = vehicle_gps_position_sub, .events = POLLIN }
};
if (!thread_should_exit) {
@ -330,7 +324,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
while (!thread_should_exit) {
int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate
int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
hrt_abstime t = hrt_absolute_time();
if (ret < 0) {
@ -340,36 +334,42 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
continue;
} else if (ret > 0) {
/* act on attitude updates */
/* vehicle attitude */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
bool updated;
/* parameter update */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
orb_check(parameter_update_sub, &updated);
if (updated) {
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub,
&update);
/* update parameters */
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
parameters_update(&pos_inav_param_handles, &params);
}
/* actuator */
if (fds[1].revents & POLLIN) {
orb_check(actuator_sub, &updated);
if (updated) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
}
/* armed */
if (fds[2].revents & POLLIN) {
orb_check(armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
}
/* vehicle attitude */
if (fds[3].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
}
/* sensor combined */
if (fds[4].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
orb_check(sensor_combined_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_counter > accel_counter) {
if (att.R_valid) {
/* correct accel bias, now only for Z */
@ -406,7 +406,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* optical flow */
if (fds[5].revents & POLLIN) {
orb_check(optical_flow_sub, &updated);
if (updated) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && (flow.ground_distance_m != sonar_prev || t < sonar_time + sonar_timeout)) {
@ -488,7 +489,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* vehicle GPS position */
if (fds[6].revents & POLLIN) {
orb_check(vehicle_gps_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
if (gps.fix_type >= 3) {