diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-12 01:44:21 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-12 01:44:21 +0200 |
commit | 43019ba618d45c5f2cc064f5dd04ee83bbeea4be (patch) | |
tree | f9bbc111fd723253bf16ff4f0567b76f217e541c /apps | |
parent | 5adb691f897f2c725dbe1665c54b06ec924af6de (diff) | |
download | px4-firmware-43019ba618d45c5f2cc064f5dd04ee83bbeea4be.tar.gz px4-firmware-43019ba618d45c5f2cc064f5dd04ee83bbeea4be.tar.bz2 px4-firmware-43019ba618d45c5f2cc064f5dd04ee83bbeea4be.zip |
Further cleanups, added sanity check against system state machine
Diffstat (limited to 'apps')
-rw-r--r-- | apps/ardrone_control/ardrone_control.c | 59 | ||||
-rw-r--r-- | apps/ardrone_control/attitude_control.c | 17 | ||||
-rw-r--r-- | apps/ardrone_control/attitude_control.h | 14 | ||||
-rw-r--r-- | apps/ardrone_control/rate_control.c | 7 | ||||
-rw-r--r-- | apps/ardrone_control/rate_control.h | 4 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 2 |
6 files changed, 60 insertions, 43 deletions
diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c index a9b04f592..e9a48bbfe 100644 --- a/apps/ardrone_control/ardrone_control.c +++ b/apps/ardrone_control/ardrone_control.c @@ -186,31 +186,40 @@ int ardrone_control_main(int argc, char *argv[]) int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint)); while (1) { - if (control_mode == CONTROL_MODE_RATES) { - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint); - control_rates(ardrone_write, &raw, &setpoint); - - } else if (control_mode == CONTROL_MODE_ATTITUDE) { - - // XXX Add failsafe logic for RC loss situations - /* hardcore, last-resort safety checking */ - //if (status->rc_signal_lost) { - - /* get a local copy of the vehicle state */ - 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 */ - 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); - - att_sp.roll_body = -manual.roll * M_PI_F / 8.0f; - att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f; - att_sp.yaw_body = -manual.yaw * M_PI_F; - - control_attitude(ardrone_write, &att_sp, &att, &state); + + if (state.state_machine == SYSTEM_STATE_MANUAL || + state.state_machine == SYSTEM_STATE_GROUND_READY || + state.state_machine == SYSTEM_STATE_STABILIZED || + state.state_machine == SYSTEM_STATE_AUTO || + state.state_machine == SYSTEM_STATE_MISSION_ABORT || + state.state_machine == SYSTEM_STATE_EMCY_LANDING) { + + if (control_mode == CONTROL_MODE_RATES) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint); + control_rates(ardrone_write, &raw, &setpoint); + + } else if (control_mode == CONTROL_MODE_ATTITUDE) { + + // XXX Add failsafe logic for RC loss situations + /* hardcore, last-resort safety checking */ + //if (status->rc_signal_lost) { + + /* get a local copy of the vehicle state */ + 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 */ + 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); + + att_sp.roll_body = -manual.roll * M_PI_F / 8.0f; + att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f; + att_sp.yaw_body = -manual.yaw * M_PI_F; + + control_attitude(ardrone_write, &att_sp, &att, &state); + } } if (counter % 30 == 0) { diff --git a/apps/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c index db82a206e..034a9c5a6 100644 --- a/apps/ardrone_control/attitude_control.c +++ b/apps/ardrone_control/attitude_control.c @@ -1,12 +1,12 @@ /**************************************************************************** * * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Laurens Mackay <mackayl@student.ethz.ch> - * Tobias Naegeli <naegelit@student.ethz.ch> - * Martin Rutschmann <rutmarti@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.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 @@ -38,7 +38,8 @@ ****************************************************************************/ /* - * @file Implementation of attitude controller + * @file attitude_control.c + * Implementation of attitude controller */ #include "attitude_control.h" @@ -331,7 +332,7 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_ /* send motors via UART */ if (motor_skip_counter % 5 == 0) { - if (motor_skip_counter % 25) printf("mot: %1.3f-%i\n", motor_thrust, motor_pwm[0]); + if (motor_skip_counter % 50) printf("mot: %1.3f-%i-%i-%i-%i\n", motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); uint8_t buf[5] = {1, 2, 3, 4, 5}; ar_get_motor_packet(buf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); write(ardrone_write, buf, sizeof(buf)); diff --git a/apps/ardrone_control/attitude_control.h b/apps/ardrone_control/attitude_control.h index a7b01268b..1067d362c 100644 --- a/apps/ardrone_control/attitude_control.h +++ b/apps/ardrone_control/attitude_control.h @@ -1,9 +1,12 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Laurens Mackay <mackayl@student.ethz.ch> + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Martin Rutschmann <rutmarti@student.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 @@ -34,7 +37,10 @@ * ****************************************************************************/ -/* @file attitude control for quadrotors */ +/* + * @file attitude_control.h + * attitude control for multi rotors + */ #ifndef ATTITUDE_CONTROL_H_ #define ATTITUDE_CONTROL_H_ diff --git a/apps/ardrone_control/rate_control.c b/apps/ardrone_control/rate_control.c index d2bedadf2..df20cd1ab 100644 --- a/apps/ardrone_control/rate_control.c +++ b/apps/ardrone_control/rate_control.c @@ -1,8 +1,8 @@ /**************************************************************************** * * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <nagelit@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Tobias Naegeli <nagelit@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,7 +34,8 @@ ****************************************************************************/ /* - * @file Implementation of attitude rate control + * @file rate_control.c + * Implementation of attitude rate control */ #include "rate_control.h" diff --git a/apps/ardrone_control/rate_control.h b/apps/ardrone_control/rate_control.h index bd3191c1c..61bd7bf38 100644 --- a/apps/ardrone_control/rate_control.h +++ b/apps/ardrone_control/rate_control.h @@ -1,8 +1,8 @@ /**************************************************************************** * * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <nagelit@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Tobias Naegeli <nagelit@student.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/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 981520e7c..6745c0102 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -71,7 +71,7 @@ #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_global_position_setpoint.h> - #include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> #include "waypoints.h" #include "mavlink_log.h" |