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 28 Packages Projects Releases Wiki Activity

[rotorcraft] fixes for ardrone2_sdk

Browse Source
This commit is contained in:
Felix Ruess
2013-12-18 17:23:38 +01:00
parent 2d5cde0a91
commit cfd143a187
3 changed files with 17 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
+13 -2
View File
@@ -28,6 +28,7 @@
* software onboard or just does not need it at all.
*/
#include "state.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "paparazzi.h"
@@ -78,7 +79,17 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
}
void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers));
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) {
memcpy(&stab_att_sp_euler, rpy, sizeof(struct Int32Eulers));
}
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) {
/* Rotate horizontal commands to body frame by psi */
int32_t psi = stateGetNedToBodyEulers_i()->psi;
int32_t s_psi, c_psi;
PPRZ_ITRIG_SIN(s_psi, psi);
PPRZ_ITRIG_COS(c_psi, psi);
stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
stab_att_sp_euler.psi = heading;
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h
+2
View File
@@ -25,4 +25,6 @@
#include "math/pprz_algebra_int.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
extern struct Int32Eulers stab_att_sp_euler;
#endif /* STABILIZATION_ATTITUDE_PASSTHROUGH_H */
sw/airborne/subsystems/ins/ins_ardrone2.h
+2 -2
View File
@@ -40,8 +40,8 @@ struct InsArdrone2 {
/* output LTP NED */
struct NedCoor_i ltp_pos;
struct NedCoor_i ltp_speed;
struct NedCoor_i ltp_accel;
struct NedCoor_f ltp_speed;
struct NedCoor_f ltp_accel;
};
extern struct InsArdrone2 ins_impl;