Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

Further cleanups, added sanity check against system state machine

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-12 01:44:21 +02:00
parent 5adb691f89
commit 43019ba618
6 changed files with 55 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_control/ardrone_control.c
+9
View File
@@ -186,6 +186,14 @@ int ardrone_control_main(int argc, char *argv[])
int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
while (1) { while (1) {
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) { if (control_mode == CONTROL_MODE_RATES) {
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint); orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint);
@@ -212,6 +220,7 @@ int ardrone_control_main(int argc, char *argv[])
control_attitude(ardrone_write, &att_sp, &att, &state); control_attitude(ardrone_write, &att_sp, &att, &state);
} }
}
if (counter % 30 == 0) { if (counter % 30 == 0) {
if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
apps/ardrone_control/attitude_control.c
+9 -8
View File
@@ -1,12 +1,12 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch>
* Laurens Mackay <mackayl@student.ethz.ch> * @author Laurens Mackay <mackayl@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch> * @author Tobias Naegeli <naegelit@student.ethz.ch>
* Martin Rutschmann <rutmarti@student.ethz.ch> * @author Martin Rutschmann <rutmarti@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * 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" #include "attitude_control.h"
@@ -331,7 +332,7 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
/* send motors via UART */ /* send motors via UART */
if (motor_skip_counter % 5 == 0) { 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}; 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]); ar_get_motor_packet(buf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
write(ardrone_write, buf, sizeof(buf)); write(ardrone_write, buf, sizeof(buf));
apps/ardrone_control/attitude_control.h
+10 -4
View File
@@ -1,9 +1,12 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * 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_ #ifndef ATTITUDE_CONTROL_H_
#define ATTITUDE_CONTROL_H_ #define ATTITUDE_CONTROL_H_
apps/ardrone_control/rate_control.c
+4 -3
View File
@@ -1,8 +1,8 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <nagelit@student.ethz.ch> * Author: @author Lorenz Meier <lm@inf.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch> * @author Tobias Naegeli <nagelit@student.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * 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" #include "rate_control.h"
apps/ardrone_control/rate_control.h
+2 -2
View File
@@ -1,8 +1,8 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <nagelit@student.ethz.ch> * Author: @author Lorenz Meier <lm@inf.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch> * @author Tobias Naegeli <nagelit@student.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions