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

Merge branch 'fixedwing_l1' of github.com:PX4/Firmware into fixedwing_l1

This commit is contained in:
Lorenz Meier 2013-09-08 21:50:14 +02:00
parent 11e4fbc374 d3ac8c9ff3
commit 8bd018c561
14 changed files with 234 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ROMFS/px4fmu_common/init.d/rc.hil
View File

@ -5,10 +5,9 @@
echo "[HIL] starting.."
uorb start
# Tell MAVLink that this link is "fast"
mavlink start -b 230400 -d /dev/ttyS1
sleep 2
mavlink start -b 230400 -d /dev/ttyACM0
# Create a fake HIL /dev/pwm_output interface
hil mode_pwm
@ -50,7 +49,8 @@ att_pos_estimator_ekf start
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
fw_pos_control_l1 start
#fw_pos_control_l1 start
fw_att_control start
echo "[HIL] setup done, running"

8
ROMFS/px4fmu_common/init.d/rc.usb
View File

@ -21,7 +21,6 @@ if mavlink stop
then
echo "stopped other MAVLink instance"
fi
sleep 2
mavlink start -b 230400 -d /dev/ttyACM0
# Stop commander
@ -37,13 +36,6 @@ then
echo "Commander started"
fi
# Stop px4io
if px4io stop
then
echo "PX4IO stopped"
fi
sleep 1
# Start px4io if present
if px4io start
then

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

@ -101,8 +101,14 @@ then
fi
fi
# Try to get an USB console
nshterm /dev/ttyACM0 &
if param compare SYS_AUTOSTART 1000
then
sh /etc/init.d/rc.hil
set MODE custom
else
# Try to get an USB console
nshterm /dev/ttyACM0 &
fi
#
# Upgrade PX4IO firmware
@ -202,7 +208,6 @@ then
then
# Telemetry port is on both FMU boards ttyS1
mavlink start -b 57600 -d /dev/ttyS1
usleep 5000
# Start commander
commander start

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

@ -487,25 +487,27 @@ PX4IO::detect()
{
int ret;
ASSERT(_task == -1);
if (_task == -1) {
/* do regular cdev init */
ret = CDev::init();
if (ret != OK)
return ret;
/* do regular cdev init */
ret = CDev::init();
if (ret != OK)
return ret;
/* get some parameters */
unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
if (protocol != PX4IO_PROTOCOL_VERSION) {
if (protocol == _io_reg_get_error) {
log("IO not installed");
} else {
log("IO version error");
mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!");
/* get some parameters */
unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
if (protocol != PX4IO_PROTOCOL_VERSION) {
if (protocol == _io_reg_get_error) {
log("IO not installed");
} else {
log("IO version error");
mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!");
}
return -1;
}
return -1;
}
log("IO found");
return 0;

73
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
View File

@ -38,20 +38,87 @@
*/
#include "ecl_pitch_controller.h"
#include <math.h>
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
ECL_PitchController::ECL_PitchController() :
_last_run(0),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_desired_rate(0.0f)
_rate_setpoint(0.0f),
_max_deflection_rad(math::radians(45.0f))
{
}
float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed)
bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
{
return 0.0f;
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = dt_micros / 1000000;
/* lock integral for long intervals */
if (dt_micros > 500000)
lock_integrator = true;
float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_i_rate = _k_i * _tc;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
} else if (airspeed < airspeed_min) {
airspeed = airspeed_min;
}
/* calculate the offset in the rate resulting from rolling */
float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
tanf(roll) * sinf(roll)) * _roll_ff;
float pitch_error = pitch_setpoint - pitch;
/* rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;
/* add turn offset */
_rate_setpoint += turn_offset;
_rate_error = _rate_setpoint - pitch_rate;
float ilimit_scaled = 0.0f;
if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * k_i_rate * dt * scaler;
/*
* anti-windup: do not allow integrator to increase into the
* wrong direction if actuator is at limit
*/
if (_last_output < -_max_deflection_rad) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > _max_deflection_rad) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
_integrator += id;
}
/* integrator limit */
_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
/* store non-limited output */
_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler;
return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
}
void ECL_PitchController::reset_integrator()

7
src/lib/ecl/attitude_fw/ecl_pitch_controller.h
View File

@ -49,7 +49,7 @@ public:
ECL_PitchController();
float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f,
bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float aspeed = (0.0f / 0.0f));
bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
void reset_integrator();
@ -83,7 +83,7 @@ public:
}
float get_desired_rate() {
return _desired_rate;
return _rate_setpoint;
}
private:
@ -100,7 +100,8 @@ private:
float _last_output;
float _integrator;
float _rate_error;
float _desired_rate;
float _rate_setpoint;
float _max_deflection_rad;
};
#endif // ECL_PITCH_CONTROLLER_H

58
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
View File

@ -39,6 +39,11 @@
#include "../ecl.h"
#include "ecl_roll_controller.h"
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
ECL_RollController::ECL_RollController() :
_last_run(0),
@ -46,13 +51,14 @@ ECL_RollController::ECL_RollController() :
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_desired_rate(0.0f)
_rate_setpoint(0.0f),
_max_deflection_rad(math::radians(45.0f))
{
}
float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate,
float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float aspeed)
float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
{
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
@ -60,10 +66,56 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
float k_i_rate = _k_i * _tc;
/* input conditioning */
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
} else if (airspeed < airspeed_min) {
airspeed = airspeed_min;
}
float roll_error = roll_setpoint - roll;
_rate_setpoint = roll_error / _tc;
/* limit the rate */
if (_max_rate > 0.01f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
_rate_error = _rate_setpoint - roll_rate;
float ilimit_scaled = 0.0f;
return 0.0f;
if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * k_i_rate * dt * scaler;
/*
* anti-windup: do not allow integrator to increase into the
* wrong direction if actuator is at limit
*/
if (_last_output < -_max_deflection_rad) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
} else if (_last_output > _max_deflection_rad) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
_integrator += id;
}
/* integrator limit */
_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
/* store non-limited output */
_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;
return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
}
void ECL_RollController::reset_integrator()

7
src/lib/ecl/attitude_fw/ecl_roll_controller.h
View File

@ -49,7 +49,7 @@ public:
ECL_RollController();
float control(float roll_setpoint, float roll, float roll_rate,
float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float aspeed = (0.0f / 0.0f));
float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
void reset_integrator();
@ -79,7 +79,7 @@ public:
}
float get_desired_rate() {
return _desired_rate;
return _rate_setpoint;
}
private:
@ -93,7 +93,8 @@ private:
float _last_output;
float _integrator;
float _rate_error;
float _desired_rate;
float _rate_setpoint;
float _max_deflection_rad;
};
#endif // ECL_ROLL_CONTROLLER_H

11
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
View File

@ -38,6 +38,11 @@
*/
#include "ecl_yaw_controller.h"
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
ECL_YawController::ECL_YawController() :
_last_run(0),
@ -54,6 +59,12 @@ ECL_YawController::ECL_YawController() :
float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator,
float airspeed_min, float airspeed_max, float aspeed)
{
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
return 0.0f;
}

55
src/modules/commander/commander.cpp
View File

@ -144,8 +144,8 @@ static int mavlink_fd;
/* flags */
static bool commander_initialized = false;
static bool thread_should_exit = false; /**< daemon exit flag */
static bool thread_running = false; /**< daemon status flag */
static volatile bool thread_should_exit = false; /**< daemon exit flag */
static volatile bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
static unsigned int leds_counter;
@ -230,7 +230,7 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("commander already running\n");
warnx("commander already running");
/* this is not an error */
exit(0);
}
@ -242,21 +242,38 @@ int commander_main(int argc, char *argv[])
3000,
commander_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
while (!thread_running) {
usleep(200);
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (!thread_running)
errx(0, "commander already stopped");
thread_should_exit = true;
while (thread_running) {
usleep(200000);
warnx(".");
}
warnx("terminated.");
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("\tcommander is running\n");
warnx("\tcommander is running");
print_status();
} else {
warnx("\tcommander not started\n");
warnx("\tcommander not started");
}
exit(0);
@ -326,6 +343,9 @@ void print_status()
warnx("arming: %s", armed_str);
}
static orb_advert_t control_mode_pub;
static orb_advert_t status_pub;
void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
{
/* result of the command */
@ -339,6 +359,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
uint8_t base_mode = (uint8_t) cmd->param1;
uint8_t custom_main_mode = (uint8_t) cmd->param2;
/* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
// TODO remove debug code
//mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
/* set arming state */
@ -526,7 +550,6 @@ int commander_thread_main(int argc, char *argv[])
}
/* Main state machine */
orb_advert_t status_pub;
/* make sure we are in preflight state */
memset(&status, 0, sizeof(status));
status.condition_landed = true; // initialize to safe value
@ -538,7 +561,7 @@ int commander_thread_main(int argc, char *argv[])
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
orb_advert_t control_mode_pub;
/* Initialize all flags to false */
memset(&control_mode, 0, sizeof(control_mode));
@ -595,16 +618,20 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_info(mavlink_fd, "[cmd] started");
int ret;
pthread_attr_t commander_low_prio_attr;
pthread_attr_init(&commander_low_prio_attr);
pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
struct sched_param param;
(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
pthread_attr_destroy(&commander_low_prio_attr);
/* Start monitoring loop */
unsigned counter = 0;
@ -1200,7 +1227,12 @@ int commander_thread_main(int argc, char *argv[])
}
/* wait for threads to complete */
pthread_join(commander_low_prio_thread, NULL);
ret = pthread_join(commander_low_prio_thread, NULL);
if (ret) {
warn("join failed", ret);
}
rgbled_set_mode(RGBLED_MODE_OFF);
/* close fds */
led_deinit();
@ -1218,9 +1250,6 @@ int commander_thread_main(int argc, char *argv[])
close(param_changed_sub);
close(battery_sub);
warnx("exiting");
fflush(stdout);
thread_running = false;
return 0;
@ -1628,7 +1657,7 @@ void *commander_low_prio_loop(void *arg)
while (!thread_should_exit) {
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
@ -1785,5 +1814,5 @@ void *commander_low_prio_loop(void *arg)
close(cmd_sub);
return 0;
return NULL;
}

2
src/modules/commander/state_machine_helper.cpp
View File

@ -504,6 +504,8 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
current_control_mode->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
// XXX also set lockdown here
ret = OK;
} else {

16
src/modules/mavlink/mavlink.c
View File

@ -743,7 +743,7 @@ int mavlink_thread_main(int argc, char *argv[])
thread_running = false;
exit(0);
return 0;
}
static void
@ -767,7 +767,7 @@ int mavlink_main(int argc, char *argv[])
/* this is not an error */
if (thread_running)
errx(0, "mavlink already running\n");
errx(0, "mavlink already running");
thread_should_exit = false;
mavlink_task = task_spawn_cmd("mavlink",
@ -776,15 +776,25 @@ int mavlink_main(int argc, char *argv[])
2048,
mavlink_thread_main,
(const char **)argv);
while (!thread_running) {
usleep(200);
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
/* this is not an error */
if (!thread_running)
errx(0, "mavlink already stopped");
thread_should_exit = true;
while (thread_running) {
usleep(200000);
printf(".");
warnx(".");
}
warnx("terminated.");

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -755,5 +755,7 @@ receive_start(int uart)
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
pthread_attr_destroy(&receiveloop_attr);
return thread;
}

2
src/modules/mavlink/orb_listener.c
View File

@ -829,5 +829,7 @@ uorb_receive_start(void)
pthread_t thread;
pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
pthread_attr_destroy(&uorb_attr);
return thread;
}