From 43019ba618d45c5f2cc064f5dd04ee83bbeea4be Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Aug 2012 01:44:21 +0200 Subject: Further cleanups, added sanity check against system state machine --- apps/ardrone_control/ardrone_control.c | 59 +++++++++++++++++++-------------- apps/ardrone_control/attitude_control.c | 17 +++++----- apps/ardrone_control/attitude_control.h | 14 +++++--- apps/ardrone_control/rate_control.c | 7 ++-- apps/ardrone_control/rate_control.h | 4 +-- 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 - * Julian Oes - * Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Lorenz Meier + * Author: @author Thomas Gubler + * @author Julian Oes + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * @author Lorenz Meier * * 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 - * Julian Oes - * + * Author: @author Thomas Gubler + * @author Julian Oes + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * @author Lorenz Meier * * 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 - * Lorenz Meier + * Author: @author Lorenz Meier + * @author Tobias Naegeli * * 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 - * Lorenz Meier + * Author: @author Lorenz Meier + * @author Tobias Naegeli * * 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 #include #include - #include +#include #include "waypoints.h" #include "mavlink_log.h" -- cgit v1.2.3