Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[module-ctrl] Generic framework to add many new types of controllers in a module

Browse Source
This commit is contained in:
dewagter
2015-01-15 11:51:16 +01:00
parent 0ab7a71bef
commit 7a0f034bd8
13 changed files with 145 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ardrone2_opticflow_hover.xml
+1 -1
View File
@@ -216,7 +216,7 @@
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_MODULE_OUTERLOOP"/>
<define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
conf/modules/cv_opticflow.xml
-1
View File
@@ -44,7 +44,6 @@
<makefile target="ap">
<define name="ARDRONE_VIDEO_PORT" value="2002" />
<define name="USE_ARDRONE_VIDEO" />
<define name="USE_MODULE_OUTERLOOP" value="1" />
<file name="opticflow_module.c"/>
<file name="opticflow_code.c" dir="modules/computer_vision/opticflow"/>
sw/airborne/firmwares/rotorcraft/autopilot.c
+8 -4
View File
@@ -423,8 +423,10 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
case AP_MODE_NAV:
guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
break;
case AP_MODE_MODULE_OUTERLOOP:
guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_OUTERLOOP);
case AP_MODE_MODULE:
#ifdef GUIDANCE_H_MODE_MODULE_SETTING
guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_SETTING);
#endif
break;
default:
break;
@@ -467,8 +469,10 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
case AP_MODE_NAV:
guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
break;
case AP_MODE_MODULE_OUTERLOOP:
guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_OUTERLOOP);
case AP_MODE_MODULE:
#ifdef GUIDANCE_V_MODE_MODULE_SETTING
guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE);
#endif
break;
default:
break;
sw/airborne/firmwares/rotorcraft/autopilot.h
+1 -1
View File
@@ -50,7 +50,7 @@
#define AP_MODE_RC_DIRECT 14 // Safety Pilot Direct Commands for helicopter direct control
#define AP_MODE_CARE_FREE_DIRECT 15
#define AP_MODE_FORWARD 16
#define AP_MODE_MODULE_OUTERLOOP 17
#define AP_MODE_MODULE 17
extern uint8_t autopilot_mode;
extern uint8_t autopilot_mode_auto2;
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+17 -9
View File
@@ -27,6 +27,7 @@
#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/guidance/guidance_module.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "firmwares/rotorcraft/navigation.h"
@@ -252,7 +253,12 @@ void guidance_h_mode_changed(uint8_t new_mode)
stabilization_attitude_enter();
break;
case GUIDANCE_H_MODE_MODULE_OUTERLOOP:
#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
case GUIDANCE_H_MODE_MODULE:
guidance_h_module_enter();
break;
#endif
case GUIDANCE_H_MODE_NAV:
guidance_h_nav_enter();
#if NO_ATTITUDE_RESET_ON_MODE_CHANGE
@@ -305,7 +311,12 @@ void guidance_h_read_rc(bool_t in_flight)
#endif
break;
case GUIDANCE_H_MODE_MODULE_OUTERLOOP:
#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
case GUIDANCE_H_MODE_MODULE:
guidance_h_module_read_rc();
break;
#endif
case GUIDANCE_H_MODE_NAV:
if (radio_control.status == RC_OK) {
stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight, FALSE, FALSE);
@@ -388,15 +399,12 @@ void guidance_h_run(bool_t in_flight)
}
stabilization_attitude_run(in_flight);
break;
case GUIDANCE_H_MODE_MODULE_OUTERLOOP:
if (!in_flight) {
guidance_h_nav_enter();
#if USE_MODULE_OUTERLOOP==1
guidance_module_run(in_flight);
#endif
}
#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
case GUIDANCE_H_MODE_MODULE:
guidance_h_module_run(in_flight);
break;
#endif
default:
break;
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+9 -9
View File
@@ -49,15 +49,15 @@
#define GUIDANCE_H_USE_SPEED_REF TRUE
#endif
#define GUIDANCE_H_MODE_KILL 0
#define GUIDANCE_H_MODE_RATE 1
#define GUIDANCE_H_MODE_ATTITUDE 2
#define GUIDANCE_H_MODE_HOVER 3
#define GUIDANCE_H_MODE_NAV 4
#define GUIDANCE_H_MODE_RC_DIRECT 5
#define GUIDANCE_H_MODE_CARE_FREE 6
#define GUIDANCE_H_MODE_FORWARD 7
#define GUIDANCE_H_MODE_MODULE_OUTERLOOP 8
#define GUIDANCE_H_MODE_KILL 0
#define GUIDANCE_H_MODE_RATE 1
#define GUIDANCE_H_MODE_ATTITUDE 2
#define GUIDANCE_H_MODE_HOVER 3
#define GUIDANCE_H_MODE_NAV 4
#define GUIDANCE_H_MODE_RC_DIRECT 5
#define GUIDANCE_H_MODE_CARE_FREE 6
#define GUIDANCE_H_MODE_FORWARD 7
#define GUIDANCE_H_MODE_MODULE 8
extern uint8_t guidance_h_mode;
sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h
+51
View File
@@ -0,0 +1,51 @@
/*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/** @file firmwares/rotorcraft/guidance/guidance_module.h
* Guidance in a module file.
*
* If the module implements both V and H mode, take into account that the H is called first and later V
*/
#ifndef GUIDANCE_MODULE_H_
#define GUIDANCE_MODULE_H_
#include "generated/modules.h"
/**
*
* Horizontal loop default is ATTITUDE
* e.g.: #define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_ATTITUDE
*
* Implement own Horizontal loops when GUIDANCE_H_MODE_MODULE_SETTING is set to GUIDANCE_H_MODE_MODULE
* extern void guidance_h_module_enter(void);
* extern void guidance_h_module_read_rc(void);
* extern void guidance_h_module_run(bool_t in_flight);
*
* Vertical loop default is ATTITUDE
* e.g.: #define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_RC_DIRECT
*
* Implement own Vertical loops when GUIDANCE_V_MODE_MODULE_SETTING is set to GUIDANCE_V_MODE_MODULE
* extern void guidance_v_module_enter(void);
* extern void guidance_v_module_run(bool_t in_flight);
*/
#endif /* GUIDANCE_MODULE_H_ */
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+12 -13
View File
@@ -26,6 +26,7 @@
#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/guidance/guidance_module.h"
#include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/stabilization.h"
@@ -232,12 +233,17 @@ void guidance_v_mode_changed(uint8_t new_mode)
case GUIDANCE_V_MODE_RC_CLIMB:
case GUIDANCE_V_MODE_CLIMB:
guidance_v_zd_sp = 0;
case GUIDANCE_V_MODE_MODULE_OUTERLOOP:
case GUIDANCE_V_MODE_NAV:
guidance_v_z_sum_err = 0;
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
break;
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
case GUIDANCE_V_MODE_MODULE:
guidance_v_module_enter();
break;
#endif
default:
break;
@@ -308,19 +314,12 @@ void guidance_v_run(bool_t in_flight)
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
break;
case GUIDANCE_V_MODE_MODULE_OUTERLOOP:
guidance_v_z_sp = -nav_flight_altitude;
guidance_v_zd_sp = 0;
gv_update_ref_from_z_sp(guidance_v_z_sp);
run_hover_loop(in_flight);
#if !NO_RC_THRUST_LIMIT
/* use rc limitation if available */
if (radio_control.status == RC_OK) {
stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t);
} else
#endif
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
case GUIDANCE_V_MODE_MODULE:
guidance_v_module_run(in_flight);
break;
#endif
case GUIDANCE_V_MODE_NAV: {
if (vertical_mode == VERTICAL_MODE_ALT) {
guidance_v_z_sp = -nav_flight_altitude;
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+7 -7
View File
@@ -32,13 +32,13 @@
#include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
#define GUIDANCE_V_MODE_KILL 0
#define GUIDANCE_V_MODE_RC_DIRECT 1
#define GUIDANCE_V_MODE_RC_CLIMB 2
#define GUIDANCE_V_MODE_CLIMB 3
#define GUIDANCE_V_MODE_HOVER 4
#define GUIDANCE_V_MODE_NAV 5
#define GUIDANCE_V_MODE_MODULE_OUTERLOOP 6
#define GUIDANCE_V_MODE_KILL 0
#define GUIDANCE_V_MODE_RC_DIRECT 1
#define GUIDANCE_V_MODE_RC_CLIMB 2
#define GUIDANCE_V_MODE_CLIMB 3
#define GUIDANCE_V_MODE_HOVER 4
#define GUIDANCE_V_MODE_NAV 5
#define GUIDANCE_V_MODE_MODULE 6
extern uint8_t guidance_v_mode;
sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c
+22 -4
View File
@@ -98,6 +98,27 @@ unsigned int set_heading;
#define VISION_DESIRED_VY 0.
#endif
void guidance_h_module_enter(void)
{
// INIT
Velx_Int = 0;
Vely_Int = 0;
// GUIDANCE: Set Hover-z-hold
guidance_v_z_sp = -1;
}
void guidance_h_module_read_rc(void)
{
// Do not read RC
// Setpoint being set by vision
}
void guidance_h_module_run(bool_t in_flight)
{
// Run
// Setpoint being set by vision
stabilization_attitude_run(in_flight);
}
void init_hover_stabilization_onvision()
@@ -122,11 +143,8 @@ void init_hover_stabilization_onvision()
void run_hover_stabilization_onvision(void)
{
if (autopilot_mode == AP_MODE_MODULE_OUTERLOOP) {
if (autopilot_mode == AP_MODE_MODULE) {
run_opticflow_hover();
} else {
Velx_Int = 0;
Vely_Int = 0;
}
}
sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h
+14
View File
@@ -31,6 +31,20 @@
#include <std.h>
// Controller module
// Vertical loop re-uses Alt-hold
#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_HOVER
// Horizontal mode is a specific controller
#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE
// Implement own Horizontal loops
extern void guidance_h_module_enter(void);
extern void guidance_h_module_read_rc(void);
extern void guidance_h_module_run(bool_t in_flight);
void init_hover_stabilization_onvision(void);
void run_hover_stabilization_onvision(void);
void run_opticflow_hover(void);
sw/airborne/modules/computer_vision/opticflow_module.c
-10
View File
@@ -30,9 +30,6 @@
// Own header
#include "opticflow_module.h"
// Navigate Based On Vision
#include "opticflow/hover_stabilization.h"
// Paparazzi
#include "state.h" // for attitude
#include "boards/ardrone/navdata.h" // for ultrasound Height
@@ -96,13 +93,6 @@ void opticflow_module_run(void)
}
}
#include "firmwares/rotorcraft/navigation.h"
void guidance_module_run(bool_t inflight)
{
nav_flight_altitude = -1;
}
/////////////////////////////////////////////////////////////////////////
// COMPUTER VISION THREAD
sw/airborne/modules/computer_vision/opticflow_module.h
+3
View File
@@ -31,6 +31,9 @@
#include "std.h"
// Navigate Based On Vision
#include "opticflow/hover_stabilization.h"
// Module functions
extern void opticflow_module_init(void);
extern void opticflow_module_run(void);