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

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

Browse Source 
Conflicts:
	apps/ardrone_interface/ardrone_motor_control.c
This commit is contained in:
Julian Oes 2012-09-27 13:44:47 +02:00
commit a05c4d0504
6 changed files with 49 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
apps/ardrone_interface/ardrone_interface.c
View File

@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.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

3
apps/ardrone_interface/ardrone_motor_control.c
View File

@ -306,7 +306,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
return errcounter;
}
/*
/**
* Sets the leds on the motor controllers, 1 turns led on, 0 off.
*/
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green)
@ -373,7 +373,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
const float min_thrust = 0.02f; /**< 2% minimum thrust */
const float max_thrust = 1.0f; /**< 100% max thrust */
const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */
const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */
const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */

3
apps/ardrone_interface/ardrone_motor_control.h
View File

@ -87,4 +87,7 @@ int ar_init_motors(int ardrone_uart, int gpio);
*/
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green);
/**
* Mix motors and output actuators
*/
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators);

57
apps/commander/commander.c
View File

@ -1050,6 +1050,8 @@ int commander_thread_main(int argc, char *argv[])
memset(&current_status, 0, sizeof(current_status));
current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
current_status.flag_system_armed = false;
current_status.offboard_control_signal_found_once = false;
current_status.rc_signal_found_once = false;
/* advertise to ORB */
stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
@ -1129,8 +1131,16 @@ int commander_thread_main(int argc, char *argv[])
while (!thread_should_exit) {
/* Get current values */
orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
bool new_data;
orb_check(sp_man_sub, &new_data);
if (new_data) {
orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
}
orb_check(sp_offboard_sub, &new_data);
if (new_data) {
orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
}
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
@ -1300,9 +1310,8 @@ int commander_thread_main(int argc, char *argv[])
/* ignore RC signals if in offboard control mode */
if (!current_status.offboard_control_signal_found_once) {
if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
/* Start RC state check */
bool prev_lost = current_status.rc_signal_lost;
if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
@ -1355,38 +1364,33 @@ int commander_thread_main(int argc, char *argv[])
if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!");
}
current_status.rc_signal_cutting_off = false;
current_status.rc_signal_lost = false;
current_status.rc_signal_lost_interval = 0;
} else {
static uint64_t last_print_time = 0;
/* print error message for first RC glitch and then every 5 s / 5000 ms) */
if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) {
if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) {
/* only complain if the offboard control is NOT active */
current_status.rc_signal_cutting_off = true;
mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!");
last_print_time = hrt_absolute_time();
}
/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
current_status.rc_signal_cutting_off = true;
current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp;
/* if the RC signal is gone for a full second, consider it lost */
if (current_status.rc_signal_lost_interval > 1000000) {
current_status.rc_signal_lost = true;
current_status.failsave_lowlevel = true;
state_changed = true;
}
// if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) {
// publish_armed_status(&current_status);
// }
}
/* Check if this is the first loss or first gain*/
if ((!prev_lost && current_status.rc_signal_lost) ||
(prev_lost && !current_status.rc_signal_lost)) {
/* publish change */
publish_armed_status(&current_status);
}
}
@ -1398,23 +1402,29 @@ int commander_thread_main(int argc, char *argv[])
/* State machine update for offboard control */
if (!current_status.rc_signal_found_once) {
if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) {
if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) {
/* decide about attitude control flag, enable in att/pos/vel */
bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
/* decide about rate control flag, enable it always XXX (for now) */
bool rates_ctrl_enabled = true;
/* set up control mode */
if (current_status.flag_control_attitude_enabled !=
(sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE)) {
current_status.flag_control_attitude_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE);
if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
state_changed = true;
}
if (current_status.flag_control_rates_enabled !=
(sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES)) {
current_status.flag_control_attitude_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES);
if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
current_status.flag_control_rates_enabled = rates_ctrl_enabled;
state_changed = true;
}
/* handle the case where RC signal was regained */
/* handle the case where offboard control signal was regained */
if (!current_status.offboard_control_signal_found_once) {
current_status.offboard_control_signal_found_once = true;
/* enable offboard control, disable manual input */
@ -1430,6 +1440,7 @@ int commander_thread_main(int argc, char *argv[])
}
}
current_status.offboard_control_signal_weak = false;
current_status.offboard_control_signal_lost = false;
current_status.offboard_control_signal_lost_interval = 0;
@ -1445,8 +1456,8 @@ int commander_thread_main(int argc, char *argv[])
} else {
static uint64_t last_print_time = 0;
/* print error message for first RC glitch and then every 5 s / 5000 ms) */
if (!current_status.offboard_control_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) {
/* only complain if the RC control is NOT active */
if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) {
current_status.offboard_control_signal_weak = true;
mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO OFFBOARD CONTROL SIGNAL!");
last_print_time = hrt_absolute_time();
}

14
apps/multirotor_att_control/multirotor_att_control_main.c
View File

@ -79,6 +79,8 @@ static bool motor_test_mode = false;
static orb_advert_t actuator_pub;
static struct vehicle_status_s state;
/**
* Perform rate control right after gyro reading
*/
@ -120,7 +122,7 @@ static void *rate_control_thread_main(void *arg)
/* perform local lowpass */
/* apply controller */
// if (state.flag_control_rates_enabled) {
if (state.flag_control_rates_enabled) {
/* lowpass gyros */
// XXX
gyro_lp[0] = gyro_report.x;
@ -129,7 +131,7 @@ static void *rate_control_thread_main(void *arg)
multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
// }
}
}
}
@ -140,7 +142,6 @@ static int
mc_thread_main(int argc, char *argv[])
{
/* declare and safely initialize all structs */
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
@ -205,7 +206,11 @@ mc_thread_main(int argc, char *argv[])
perf_begin(mc_loop_perf);
/* get a local copy of system state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
bool updated;
orb_check(state_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
}
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */
@ -213,7 +218,6 @@ mc_thread_main(int argc, char *argv[])
/* 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(offboard_control_setpoint), setpoint_sub, &offboard_sp);

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

@ -143,6 +143,7 @@ struct vehicle_status_s
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;
bool offboard_control_signal_weak;
uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */
bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */