Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Care Free mode

Browse Source
This commit is contained in:
Ewoud Smeur
2013-03-08 17:22:56 +01:00
parent 4733bc45c0
commit 7636a963fc
7 changed files with 23 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/esden/gain_scheduling_example.xml
-2
View File
@@ -210,8 +210,6 @@
<subsystem name="radio_control" type="ppm">
<configure name="RADIO_CONTROL_PPM_PIN" value="UART1_RX"/>
</subsystem>
<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
</target>
<target name="nps" board="pc">
sw/airborne/firmwares/rotorcraft/autopilot.c
+5
View File
@@ -150,6 +150,10 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
case AP_MODE_RATE_Z_HOLD:
guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
break;
case AP_MODE_CARE_FREE:
//Take the current psi as the reference for pitch and roll
care_free_heading = stateGetNedToBodyEulers_f()->psi;
case AP_MODE_ATTITUDE_RC_CLIMB:
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_ATTITUDE_Z_HOLD:
@@ -181,6 +185,7 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
case AP_MODE_RATE_DIRECT:
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_HOVER_DIRECT:
case AP_MODE_CARE_FREE:
guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT);
break;
case AP_MODE_RATE_RC_CLIMB:
sw/airborne/firmwares/rotorcraft/autopilot.h
+2 -2
View File
@@ -49,8 +49,8 @@
#define AP_MODE_HOVER_CLIMB 10
#define AP_MODE_HOVER_Z_HOLD 11
#define AP_MODE_NAV 12
#define AP_MODE_RC_DIRECT 13 // Safety Pilot Direct Commands for helicopter direct control: appropriately chosen as mode "13"
#define AP_MODE_RC_DIRECT 13 // Safety Pilot Direct Commands for helicopter direct control: appropriately chosen as mode "13"
#define AP_MODE_CARE_FREE 14
extern uint8_t autopilot_mode;
extern uint8_t autopilot_mode_auto2;
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
+3
View File
@@ -24,6 +24,9 @@
#include STABILIZATION_ATTITUDE_TYPE_H
extern float care_free_heading;
extern void stabilization_attitude_init(void);
extern void stabilization_attitude_read_rc(bool_t in_flight);
extern void stabilization_attitude_enter(void);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+2
View File
@@ -58,6 +58,8 @@ struct Int32Eulers stabilization_att_sum_err;
int32_t stabilization_att_fb_cmd[COMMANDS_NB];
int32_t stabilization_att_ff_cmd[COMMANDS_NB];
float care_free_heading = 0;
#define IERROR_SCALE 1024
#define GAIN_PRESCALER_FF 48
#define GAIN_PRESCALER_P 48
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
+10 -1
View File
@@ -33,6 +33,7 @@
#include "subsystems/radio_control.h"
#include "state.h"
#include "firmwares/rotorcraft/autopilot.h"
#if defined STABILIZATION_ATTITUDE_TYPE_INT
#define SP_MAX_PHI (int32_t)ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI)
@@ -187,7 +188,15 @@ static inline void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQu
/* get current heading */
const struct FloatVect3 zaxis = {0., 0., 1.};
struct FloatQuat q_yaw;
FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, stateGetNedToBodyEulers_f()->psi);
//Care Free mode
if(autopilot_mode == AP_MODE_CARE_FREE) {
//care_free_heading has been set to current psi when entering care free mode.
FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, care_free_heading);
}
else {
FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, stateGetNedToBodyEulers_f()->psi);
}
/* roll/pitch commands applied to to current heading */
struct FloatQuat q_rp_sp;
sw/airborne/modules/gain_scheduling/gain_scheduling.c
+1
View File
@@ -27,6 +27,7 @@
//Include for scheduling on transition_status
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/stabilization.h"
// #include "state.h"
#include "math/pprz_algebra_int.h"