fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 14:23:34 +08:00
Code Issues 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
+29 -20
View File
@@ -186,31 +186,40 @@ 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 (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) { 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) {
// XXX Add failsafe logic for RC loss situations if (control_mode == CONTROL_MODE_RATES) {
/* hardcore, last-resort safety checking */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
//if (status->rc_signal_lost) { orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint);
control_rates(ardrone_write, &raw, &setpoint);
/* get a local copy of the vehicle state */ } else if (control_mode == CONTROL_MODE_ATTITUDE) {
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; // XXX Add failsafe logic for RC loss situations
att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f; /* hardcore, last-resort safety checking */
att_sp.yaw_body = -manual.yaw * M_PI_F; //if (status->rc_signal_lost) {
control_attitude(ardrone_write, &att_sp, &att, &state); /* 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) { if (counter % 30 == 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
apps/mavlink/mavlink.c
+1 -1
View File
@@ -71,7 +71,7 @@
#include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_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 "waypoints.h"
#include "mavlink_log.h" #include "mavlink_log.h"