diff options
author | Julian Oes <joes@student.ethz.ch> | 2012-09-27 13:44:47 +0200 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2012-09-27 13:44:47 +0200 |
commit | a05c4d050473df87aacc95aadcd978d57f263cbc (patch) | |
tree | 5ebb7fdeea0c0ce5c71b082f3fc58f2b30bc8c13 | |
parent | 8b795e7ee4d689734e6321d6e92e14eb6bffd7e7 (diff) | |
parent | ec3949bf82dbaa50ea866b65cd0fc4630af18001 (diff) | |
download | px4-firmware-a05c4d050473df87aacc95aadcd978d57f263cbc.tar.gz px4-firmware-a05c4d050473df87aacc95aadcd978d57f263cbc.tar.bz2 px4-firmware-a05c4d050473df87aacc95aadcd978d57f263cbc.zip |
Merge branch 'ardrone' of https://github.com/PX4/Firmware into ardrone
Conflicts:
apps/ardrone_interface/ardrone_motor_control.c
-rw-r--r-- | apps/ardrone_interface/ardrone_interface.c | 2 | ||||
-rw-r--r-- | apps/ardrone_interface/ardrone_motor_control.c | 3 | ||||
-rw-r--r-- | apps/ardrone_interface/ardrone_motor_control.h | 3 | ||||
-rw-r--r-- | apps/commander/commander.c | 57 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 14 | ||||
-rw-r--r-- | apps/uORB/topics/vehicle_status.h | 1 |
6 files changed, 49 insertions, 31 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index f12f9cb47..f0bc0b1e7 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -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 diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 89ed183cc..e410d3a71 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -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 */ diff --git a/apps/ardrone_interface/ardrone_motor_control.h b/apps/ardrone_interface/ardrone_motor_control.h index 664419707..78b603b63 100644 --- a/apps/ardrone_interface/ardrone_motor_control.h +++ b/apps/ardrone_interface/ardrone_motor_control.h @@ -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); diff --git a/apps/commander/commander.c b/apps/commander/commander.c index a66885b06..66a981a68 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1050,6 +1050,8 @@ int commander_thread_main(int argc, char *argv[]) memset(¤t_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), ¤t_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(¤t_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(¤t_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(); } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 904872dde..8d648c194 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -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); diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index c727527b1..23172d7cf 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -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 */ |