Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 16:30:07 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[rotorcraft] simple HITL by using ref as gps

Browse Source
This commit is contained in:
softsr
2014-02-09 04:42:47 -08:00
committed by Felix Ruess
parent 7f90cb1b3a
commit 2446fc0c9d
4 changed files with 136 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile
+15
View File
@@ -0,0 +1,15 @@
# Hey Emacs, this is a -*- makefile -*-
ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_hitl.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_hitl.c
ap.CFLAGS += -DUSE_GPS -DHITL
ifneq ($(GPS_LED),none)
ap.CFLAGS += -DGPS_LED=$(GPS_LED)
endif
nps.CFLAGS += -DUSE_GPS
nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
sw/airborne/subsystems/actuators/motor_mixing.c
+4
View File
@@ -158,7 +158,11 @@ void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter)
void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) {
uint8_t i;
#if !HITL
if (motors_on) {
#else
if (FALSE) {
#endif
int32_t min_cmd = INT32_MAX;
int32_t max_cmd = INT32_MIN;
/* do the mixing in float to avoid overflows, implicitly casted back to int32_t */
sw/airborne/subsystems/gps/gps_sim_hitl.c
+35
View File
@@ -0,0 +1,35 @@
/*
* Copyright (C) 2014 Sergey Krukowski <softsr@yahoo.de>
*
* 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 subsystems/gps/gps_sim_hitl.c
* GPS subsystem simulation from rotorcrafts horizontal/vertical reference system
*/
#include "subsystems/gps.h"
bool_t gps_available;
uint32_t gps_sim_hitl_timer;
void gps_impl_init(void) {
gps.fix = GPS_FIX_NONE;
gps_available = FALSE;
}
sw/airborne/subsystems/gps/gps_sim_hitl.h
+82
View File
@@ -0,0 +1,82 @@
/*
* Copyright (C) 2014 Sergey Krukowski <softsr@yahoo.de>
*
* 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 subsystems/gps/gps_sim_hitl.h
* GPS subsystem simulation from rotorcrafts horizontal/vertical reference system
*/
#ifndef GPS_SIM_HITL_H
#define GPS_SIM_HITL_H
#include "std.h"
#include "subsystems/ins.h"
#include "guidance/guidance_h.h"
#include "guidance/guidance_v.h"
extern bool_t gps_available;
extern uint32_t gps_sim_hitl_timer;
#define GpsEvent(_sol_available_callback) { \
if (SysTimeTimer(gps_sim_hitl_timer) > 100000) { \
SysTimeTimerStart(gps_sim_hitl_timer); \
gps.last_msg_ticks = sys_time.nb_sec_rem; \
gps.last_msg_time = sys_time.nb_sec; \
if (ins_impl.ltp_initialized) { \
struct NedCoor_i ned_coor; \
struct NedCoor_i ned_vel; \
if (!autopilot_in_flight) { \
struct Int32Vect2 zero_vector; \
INT_VECT2_ZERO(zero_vector); \
gh_set_ref(zero_vector, zero_vector, zero_vector); \
INT_VECT2_ZERO(guidance_h_pos_ref); \
INT_VECT2_ZERO(guidance_h_speed_ref); \
INT_VECT2_ZERO(guidance_h_accel_ref); \
gv_set_ref(0, 0, 0); \
guidance_v_z_ref = 0; \
guidance_v_zd_ref = 0; \
guidance_v_zdd_ref = 0; \
} \
VECT3_ASSIGN(ned_coor, guidance_h_pos_ref.x * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM, \
guidance_h_pos_ref.y * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM, \
guidance_v_z_ref * INT32_POS_OF_CM_DEN / INT32_POS_OF_CM_NUM); \
VECT3_ASSIGN(ned_vel, guidance_h_speed_ref.x * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM, \
guidance_h_speed_ref.y * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM, \
guidance_v_zd_ref * INT32_SPEED_OF_CM_S_DEN / INT32_SPEED_OF_CM_S_NUM); \
ecef_of_ned_point_i(&gps.ecef_pos, &ins_impl.ltp_def, &ned_coor); \
ecef_of_ned_vect_i(&gps.ecef_vel, &ins_impl.ltp_def, &ned_vel); \
gps.fix = GPS_FIX_3D; \
gps.last_3dfix_ticks = sys_time.nb_sec_rem; \
gps.last_3dfix_time = sys_time.nb_sec; \
gps_available = TRUE; \
} \
else { \
struct Int32Vect2 zero_vector; \
INT_VECT2_ZERO(zero_vector); \
gh_set_ref(zero_vector, zero_vector, zero_vector); \
gv_set_ref(0, 0, 0); \
} \
_sol_available_callback(); \
gps_available = FALSE; \
} \
}
#endif /* GPS_SIM_HITL_H */