fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
apps/ardrone_control/ardrone_control.c
View File

@ -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) {
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
/* hardcore, last-resort safety checking */
//if (status->rc_signal_lost) {
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);
/* 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);
} else if (control_mode == CONTROL_MODE_ATTITUDE) {
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;
// XXX Add failsafe logic for RC loss situations
/* hardcore, last-resort safety checking */
//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) {

17
apps/ardrone_control/attitude_control.c
View File

@ -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));

14
apps/ardrone_control/attitude_control.h
View File

@ -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_

7
apps/ardrone_control/rate_control.c
View File

@ -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"

4
apps/ardrone_control/rate_control.h
View File

@ -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

2
apps/mavlink/mavlink.c
View File

@ -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"