diff --git a/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile b/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile new file mode 100644 index 0000000000..c2ea1367a8 --- /dev/null +++ b/conf/firmwares/subsystems/rotorcraft/gps_sim_hitl.makefile @@ -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 diff --git a/sw/airborne/subsystems/actuators/motor_mixing.c b/sw/airborne/subsystems/actuators/motor_mixing.c index 910bc09e14..c96812cc78 100644 --- a/sw/airborne/subsystems/actuators/motor_mixing.c +++ b/sw/airborne/subsystems/actuators/motor_mixing.c @@ -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 */ diff --git a/sw/airborne/subsystems/gps/gps_sim_hitl.c b/sw/airborne/subsystems/gps/gps_sim_hitl.c new file mode 100644 index 0000000000..87ed9082a9 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_sim_hitl.c @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2014 Sergey Krukowski + * + * 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; +} diff --git a/sw/airborne/subsystems/gps/gps_sim_hitl.h b/sw/airborne/subsystems/gps/gps_sim_hitl.h new file mode 100644 index 0000000000..bbd66579ef --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_sim_hitl.h @@ -0,0 +1,82 @@ +/* + * Copyright (C) 2014 Sergey Krukowski + * + * 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 */