diff --git a/conf/airframes/tudelft/rot_wing_25kg.xml b/conf/airframes/tudelft/rot_wing_25kg.xml index 23f865050f..dd349dd45f 100644 --- a/conf/airframes/tudelft/rot_wing_25kg.xml +++ b/conf/airframes/tudelft/rot_wing_25kg.xml @@ -281,7 +281,8 @@ - + + @@ -446,7 +447,7 @@ - + diff --git a/conf/airframes/tudelft/rot_wing_v3b.xml b/conf/airframes/tudelft/rot_wing_v3b.xml index ecc5690b7a..a757334d76 100644 --- a/conf/airframes/tudelft/rot_wing_v3b.xml +++ b/conf/airframes/tudelft/rot_wing_v3b.xml @@ -338,7 +338,8 @@ - + + @@ -426,10 +427,10 @@ - + - + @@ -490,7 +491,7 @@ - + diff --git a/conf/airframes/tudelft/rot_wing_v3d.xml b/conf/airframes/tudelft/rot_wing_v3d.xml index 730822a27a..888a077243 100644 --- a/conf/airframes/tudelft/rot_wing_v3d.xml +++ b/conf/airframes/tudelft/rot_wing_v3d.xml @@ -326,7 +326,8 @@ - + + @@ -414,10 +415,10 @@ - + - + @@ -477,7 +478,7 @@ - + diff --git a/conf/airframes/tudelft/rot_wing_v3e.xml b/conf/airframes/tudelft/rot_wing_v3e.xml index f72ef8cd26..f983c536a6 100644 --- a/conf/airframes/tudelft/rot_wing_v3e.xml +++ b/conf/airframes/tudelft/rot_wing_v3e.xml @@ -326,7 +326,8 @@ - + + @@ -414,10 +415,10 @@ - + - + @@ -477,7 +478,7 @@ - + diff --git a/conf/airframes/tudelft/rot_wing_v3f.xml b/conf/airframes/tudelft/rot_wing_v3f.xml index 4ed1a1519c..0c49a0899c 100644 --- a/conf/airframes/tudelft/rot_wing_v3f.xml +++ b/conf/airframes/tudelft/rot_wing_v3f.xml @@ -326,7 +326,8 @@ - + + @@ -414,10 +415,10 @@ - + - + @@ -477,7 +478,7 @@ - + diff --git a/conf/airframes/tudelft/rot_wing_v3g.xml b/conf/airframes/tudelft/rot_wing_v3g.xml index 94353f1450..6142367873 100644 --- a/conf/airframes/tudelft/rot_wing_v3g.xml +++ b/conf/airframes/tudelft/rot_wing_v3g.xml @@ -30,6 +30,7 @@ + @@ -323,14 +324,15 @@ - + - + + @@ -339,7 +341,6 @@ -
@@ -421,10 +422,10 @@ - + - + @@ -484,7 +485,7 @@ - + @@ -502,6 +503,7 @@ +
@@ -512,12 +514,16 @@
- - + +
+
+ +
+
@@ -553,7 +559,9 @@ Check tail connection Check wings taped and secured Check attitude and heading + Airspeed sensor calibration Put UAV on take-off location + Automated actuator check Check flight plan Switch to correct flight block Switch on camera diff --git a/conf/airframes/tudelft/rot_wing_v3h.xml b/conf/airframes/tudelft/rot_wing_v3h.xml index caf1cf2d64..8f8648b082 100644 --- a/conf/airframes/tudelft/rot_wing_v3h.xml +++ b/conf/airframes/tudelft/rot_wing_v3h.xml @@ -30,6 +30,7 @@ + @@ -330,7 +331,8 @@ - + + @@ -339,7 +341,6 @@ -
@@ -354,7 +355,7 @@
- + @@ -421,10 +422,10 @@ - + - + @@ -484,7 +485,7 @@ - + diff --git a/conf/flight_plans/tudelft/rotating_wing_EHR8.xml b/conf/flight_plans/tudelft/rotating_wing_EHR8.xml index 49c2874f26..3eeea527a2 100644 --- a/conf/flight_plans/tudelft/rotating_wing_EHR8.xml +++ b/conf/flight_plans/tudelft/rotating_wing_EHR8.xml @@ -41,9 +41,14 @@ + + + + + - + @@ -59,7 +64,7 @@ - + @@ -68,16 +73,25 @@ + + - + + + + + - - + + + + + @@ -92,6 +106,13 @@ + + + + + + + - - + + @@ -41,10 +41,15 @@ + + + + + - + + + @@ -68,16 +73,25 @@ + + - + + + + + - - + + + + + @@ -162,6 +176,10 @@ --> + + + + diff --git a/conf/modules/eff_scheduling_rot_wing.xml b/conf/modules/eff_scheduling_rot_wing.xml index f436729246..0a055a22b5 100644 --- a/conf/modules/eff_scheduling_rot_wing.xml +++ b/conf/modules/eff_scheduling_rot_wing.xml @@ -22,6 +22,7 @@ + @@ -32,7 +33,7 @@ - + diff --git a/conf/modules/ins_ekf2.xml b/conf/modules/ins_ekf2.xml index 5e061d81a8..d32f9d00e8 100644 --- a/conf/modules/ins_ekf2.xml +++ b/conf/modules/ins_ekf2.xml @@ -49,7 +49,7 @@ - + diff --git a/conf/modules/parachute.xml b/conf/modules/parachute.xml index ea547beb4b..e821b27058 100644 --- a/conf/modules/parachute.xml +++ b/conf/modules/parachute.xml @@ -7,7 +7,7 @@ - + diff --git a/conf/modules/pfc_actuators.xml b/conf/modules/pfc_actuators.xml index a8d9a3eb2d..0593b54417 100644 --- a/conf/modules/pfc_actuators.xml +++ b/conf/modules/pfc_actuators.xml @@ -12,7 +12,7 @@ - + diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 9264f8ba41..fd0959c924 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -557,7 +557,7 @@ telemetry="telemetry/highspeed_rotorcraft.xml" flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml" settings="settings/rotorcraft_basic.xml" - settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml" + settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml" gui_color="red" /> + + + diff --git a/conf/userconf/tudelft/control_panel.xml b/conf/userconf/tudelft/control_panel.xml index 8703b84f77..8e7a2a12dc 100644 --- a/conf/userconf/tudelft/control_panel.xml +++ b/conf/userconf/tudelft/control_panel.xml @@ -66,6 +66,66 @@
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -138,110 +198,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -264,15 +220,19 @@ - - - - + + + + + + + + - + - + @@ -299,7 +259,7 @@ - + @@ -310,57 +270,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -368,6 +277,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -401,10 +363,33 @@ - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -449,18 +434,46 @@ - + - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c index 4861954efa..c2642ec09a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c @@ -282,6 +282,7 @@ Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT]; Butterworth2LowPass measurement_lowpass_filters[3]; Butterworth2LowPass estimation_output_lowpass_filters[3]; Butterworth2LowPass acceleration_lowpass_filter; +Butterworth2LowPass acceleration_body_x_filter; #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER Butterworth2LowPass rates_filt_so[3]; #else @@ -462,6 +463,9 @@ void init_filters(void) init_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], tau_est, sample_time, 0.0); } + // Filtering the bodyx acceleration with same cutoff as gyroscope + init_butterworth_2_low_pass(&acceleration_body_x_filter, tau, sample_time, 0.0); + // Filtering of the accel body z init_butterworth_2_low_pass(&acceleration_lowpass_filter, tau_est, sample_time, 0.0); @@ -520,11 +524,19 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s /* Propagate the filter on the gyroscopes */ struct FloatRates *body_rates = stateGetBodyRates_f(); float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r}; + + // Get the acceleration in body axes + struct Int32Vect3 *body_accel_i; + body_accel_i = stateGetAccelBody_i(); + ACCELS_FLOAT_OF_BFP(body_accel_f, *body_accel_i); + int8_t i; for (i = 0; i < 3; i++) { update_butterworth_2_low_pass(&measurement_lowpass_filters[i], rate_vect[i]); update_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], rate_vect[i]); + update_butterworth_2_low_pass(&acceleration_body_x_filter, body_accel_f.x); + //Calculate the angular acceleration via finite difference angular_acceleration[i] = (measurement_lowpass_filters[i].o[0] - measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY; @@ -620,6 +632,14 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s // This term compensates for the spinup torque in the yaw axis float g2_times_u = float_vect_dot_product(g2, indi_u, INDI_NUM_ACT)/INDI_G_SCALING; + if (in_flight) { + // Limit the estimated disturbance in yaw for drones that are stable in sideslip + BoundAbs(angular_acc_disturbance_estimate[2], stablization_indi_yaw_dist_limit); + } else { + // Not in flight, so don't estimate disturbance + float_vect_zero(angular_acc_disturbance_estimate, INDI_OUTPUTS); + } + // The control objective in array format indi_v[0] = (angular_accel_ref.p - angular_acc_disturbance_estimate[0]); indi_v[1] = (angular_accel_ref.q - angular_acc_disturbance_estimate[1]); @@ -672,7 +692,7 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s //update thrust command such that the current is correctly estimated cmd[COMMAND_THRUST] = 0; for (i = 0; i < INDI_NUM_ACT; i++) { - cmd[COMMAND_THRUST] += actuator_state[i] * -((int32_t) act_is_servo[i] - 1); + cmd[COMMAND_THRUST] += actuator_state[i] * (int32_t) act_is_thruster_z[i]; } cmd[COMMAND_THRUST] /= num_thrusters; diff --git a/sw/airborne/modules/ctrl/approach_moving_target.c b/sw/airborne/modules/ctrl/approach_moving_target.c index 55e8842757..61defc1210 100644 --- a/sw/airborne/modules/ctrl/approach_moving_target.c +++ b/sw/airborne/modules/ctrl/approach_moving_target.c @@ -201,7 +201,7 @@ void follow_diagonal_approach(void) // just filtering the heading will go wrong if it wraps, instead do: // take the diff with current heading_filtered, wrap that, add to current, insert in filter and wrap again? // example: target_heading - target_heading_filt (170 - -170 = 340), -> -20 -> -190 -> into filter -> wrap - float heading_diff = target_heading - target_heading_filt.o[0]; + float heading_diff = RadOfDeg(target_heading) - target_heading_filt.o[0]; FLOAT_ANGLE_NORMALIZE(heading_diff); float target_heading_input = target_heading_filt.o[0] + heading_diff; update_butterworth_2_low_pass(&target_heading_filt, target_heading_input); @@ -220,7 +220,7 @@ void follow_diagonal_approach(void) // Reference model float gamma_ref = RadOfDeg(amt.slope_ref); - float psi_ref = RadOfDeg(target_heading_filt_wrapped + amt.psi_ref); + float psi_ref = target_heading_filt_wrapped + RadOfDeg(amt.psi_ref); amt.rel_unit_vec.x = cosf(gamma_ref) * cosf(psi_ref); amt.rel_unit_vec.y = cosf(gamma_ref) * sinf(psi_ref); diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c index 5e789190d5..942c998128 100644 --- a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c +++ b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c @@ -30,6 +30,8 @@ #include "firmwares/rotorcraft/stabilization/stabilization_indi.h" +#include "autopilot.h" + #include "modules/actuators/actuators.h" #include "modules/core/abi.h" @@ -144,7 +146,9 @@ struct rot_wing_eff_sched_param_t eff_sched_p = { .k_lift_tail = ROT_WING_EFF_SCHED_K_LIFT_TAIL }; -float eff_sched_pusher_time = 0.002; +float eff_sched_pusher_time = 0.002; + +float roll_eff_scaling = 1.f; struct rot_wing_eff_sched_var_t eff_sched_var; @@ -282,9 +286,17 @@ void eff_scheduling_rot_wing_update_hover_motor_effectiveness(void) float pitch_motor_q_eff = eff_sched_var.pitch_motor_dMdpprz / eff_sched_var.Iyy; + float cmd_right = actuator_state_filt_vect[1]; + float cmd_left = actuator_state_filt_vect[3]; + Bound(cmd_right, 3500, MAX_PPRZ); + Bound(cmd_left, 3500, MAX_PPRZ); + // Roll motor effectiveness - float dM_dpprz_right = (eff_sched_p.DMdpprz_hover_roll[0] + actuator_state_filt_vect[1] * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; - float dM_dpprz_left = (eff_sched_p.DMdpprz_hover_roll[0] + actuator_state_filt_vect[3] * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; + float dM_dpprz_right = (eff_sched_p.DMdpprz_hover_roll[0] + cmd_right * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; + float dM_dpprz_left = (eff_sched_p.DMdpprz_hover_roll[0] + cmd_left * eff_sched_p.DMdpprz_hover_roll[1]) / 10000.; + + dM_dpprz_right = dM_dpprz_right * roll_eff_scaling; + dM_dpprz_left = dM_dpprz_left * roll_eff_scaling; // Bound dM_dpprz to half and 2 times the hover effectiveness Bound(dM_dpprz_right, eff_sched_var.roll_motor_dMdpprz * 0.5, eff_sched_var.roll_motor_dMdpprz * 2.0); @@ -294,8 +306,10 @@ void eff_scheduling_rot_wing_update_hover_motor_effectiveness(void) Bound(roll_motor_p_eff_right, -1, -0.00001); float roll_motor_p_eff_left = (dM_dpprz_left * eff_sched_var.cosr + eff_sched_p.hover_roll_roll_coef[0] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.airspeed * eff_sched_var.cosr) / eff_sched_var.Ixx; - float roll_motor_airspeed_compensation = eff_sched_p.hover_roll_roll_coef[1] * eff_sched_var.airspeed * eff_sched_var.cosr / eff_sched_var.Ixx; - roll_motor_p_eff_left += roll_motor_airspeed_compensation; + if(autopilot.in_flight) { + float roll_motor_airspeed_compensation = eff_sched_p.hover_roll_roll_coef[1] * eff_sched_var.airspeed * eff_sched_var.cosr / eff_sched_var.Ixx; + roll_motor_p_eff_left += roll_motor_airspeed_compensation; + } Bound(roll_motor_p_eff_left, 0.00001, 1); float roll_motor_q_eff = (eff_sched_p.hover_roll_pitch_coef[0] * eff_sched_var.wing_rotation_rad + eff_sched_p.hover_roll_pitch_coef[1] * eff_sched_var.wing_rotation_rad * eff_sched_var.wing_rotation_rad * eff_sched_var.sinr) / eff_sched_var.Iyy; @@ -324,6 +338,10 @@ void eff_scheduling_rot_wing_update_elevator_effectiveness(void) eff_sched_p.k_elevator[1] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_pusher_scaled * eff_sched_var.airspeed + eff_sched_p.k_elevator[2] * eff_sched_var.airspeed2) / 10000.; + // scale the effectiveness of the elevator down if it has a large deflection to encourage it to become flat quickly (pragmatic, not physically inpsired) + float elevator_ineffectiveness_scaling = (50-de)/40; + Bound(elevator_ineffectiveness_scaling, 0.5, 1.0); + float dMydpprz = dMyde * eff_sched_p.k_elevator_deflection[1]; // Convert moment to effectiveness diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.h b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.h index b80997c175..464fa97de9 100644 --- a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.h +++ b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing.h @@ -77,6 +77,8 @@ struct rot_wing_eff_sched_var_t { float airspeed2; }; +extern float roll_eff_scaling; + extern float rotation_angle_setpoint_deg; extern int16_t rotation_cmd; diff --git a/sw/airborne/modules/rot_wing_drone/rotwing_state.c b/sw/airborne/modules/rot_wing_drone/rotwing_state.c index f3c295e3b2..c22bd7ac8f 100644 --- a/sw/airborne/modules/rot_wing_drone/rotwing_state.c +++ b/sw/airborne/modules/rot_wing_drone/rotwing_state.c @@ -78,6 +78,9 @@ #endif // FW hov mot off state identification +#ifndef ROTWING_HOV_MOT_RUN_RPM_TH +#define ROTWING_HOV_MOT_RUN_RPM_TH 800 +#endif #ifndef ROTWING_HOV_MOT_OFF_RPM_TH #define ROTWING_HOV_MOT_OFF_RPM_TH 50 #endif @@ -137,6 +140,7 @@ float rotwing_state_max_hover_speed = 7; bool hover_motors_active = true; bool bool_disable_hover_motors = false; +float Wu_gih_original[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU; inline void rotwing_check_set_current_state(void); inline void rotwing_switch_state(void); @@ -220,7 +224,7 @@ void periodic_rotwing_state(void) bool_disable_hover_motors = false; } - + } // Function to request a state @@ -604,7 +608,7 @@ void rotwing_state_free_processor(void) /* Calculations - */ + */ // speed over pos_error projection struct FloatVect2 pos_error_norm; VECT2_COPY(pos_error_norm, pos_error); @@ -612,9 +616,9 @@ void rotwing_state_free_processor(void) float dist_to_target = sqrtf(nav_rotorcraft_base.goto_wp.dist2_to_wp); float max_speed_decel2 = fabsf(2.f * dist_to_target * nav_max_deceleration_sp); // dist_to_wp can only be positive, but just in case float max_speed_decel = sqrtf(max_speed_decel2); - + // Check if speed setpoint above set airspeed - struct FloatVect2 desired_airspeed_v; + struct FloatVect2 desired_airspeed_v; struct FloatVect2 groundspeed_sp; groundspeed_sp.x = pos_error.x * nav_hybrid_pos_gain; groundspeed_sp.y = pos_error.y * nav_hybrid_pos_gain; @@ -708,6 +712,18 @@ static void rotwing_state_feedback_cb(uint8_t __attribute__((unused)) sender_id, } } +bool rotwing_state_hover_motors_running(void) { + // Check if hover motors are running + if (rotwing_state_hover_rpm[0] > ROTWING_HOV_MOT_RUN_RPM_TH + && rotwing_state_hover_rpm[1] > ROTWING_HOV_MOT_RUN_RPM_TH + && rotwing_state_hover_rpm[2] > ROTWING_HOV_MOT_RUN_RPM_TH + && rotwing_state_hover_rpm[3] > ROTWING_HOV_MOT_RUN_RPM_TH) { + return true; + } else { + return false; + } +} + void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle) { // adjust weights @@ -756,9 +772,11 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, fl float pitch_angle_range = 3.; if (rotwing_state_skewing.wing_angle_deg < 55) { scheduled_pitch_angle = 0; + Wu_gih[1] = Wu_gih_original[1]; } else { float pitch_progression = (rotwing_state_skewing.wing_angle_deg - 55) / 35.; scheduled_pitch_angle = pitch_angle_range * pitch_progression; + Wu_gih[1] = Wu_gih_original[1] * (1.f - pitch_angle_range*0.9); } if (!hover_motors_active) { scheduled_pitch_angle = 8.; diff --git a/sw/airborne/modules/rot_wing_drone/rotwing_state.h b/sw/airborne/modules/rot_wing_drone/rotwing_state.h index c9faea1833..981f7977b1 100644 --- a/sw/airborne/modules/rot_wing_drone/rotwing_state.h +++ b/sw/airborne/modules/rot_wing_drone/rotwing_state.h @@ -92,5 +92,6 @@ extern void periodic_rotwing_state(void); extern void request_rotwing_state(uint8_t state); extern void rotwing_request_configuration(uint8_t configuration); extern void rotwing_state_skew_actuator_periodic(void); +extern bool rotwing_state_hover_motors_running(void); #endif // ROTWING_STATE_H diff --git a/sw/ground_segment/python/moving_base/moving_base.py b/sw/ground_segment/python/moving_base/moving_base.py index 25400a04c7..4507b43ce7 100755 --- a/sw/ground_segment/python/moving_base/moving_base.py +++ b/sw/ground_segment/python/moving_base/moving_base.py @@ -52,19 +52,22 @@ class UAV: self.timeout = 0 class Base: - def __init__(self, freq=10., use_ground_ref=False, ignore_geo_fence=False, verbose=False): + def __init__(self, freq=10., use_ground_ref=False, verbose=False, speed=0, heading=0, turn_rate=0, id=[]): self.step = 1. / freq self.use_ground_ref = use_ground_ref self.enabled = True # run sim by default self.verbose = verbose - self.ids = [5] + self.ids = [] self.uavs = [UAV(i) for i in self.ids] self.time = time.mktime(time.gmtime()) - self.speed = 1 # m/s + self.speed = speed # m/s self.course = -90 # deg - self.heading = -90 # deg - self.lat = 38.08000040764657 #deg - self.lon = -9.1 #deg + self.heading = heading # deg + self.turn_rate = turn_rate #deg/s + # self.lat = 38.08000040764657 #deg + # self.lon = -9.1 #deg + self.lat = 52.926 #deg + self.lon = 4.499 #deg self.altitude = 2.0 # starts from 1 m high # Start IVY interface @@ -72,6 +75,9 @@ class Base: # bind to GPS_INT message def ins_cb(ac_id, msg): + if ac_id not in self.ids: + self.ids.append(ac_id) + self.uavs.append(UAV(ac_id)) if ac_id in self.ids and msg.name == "GPS_INT": uav = self.uavs[self.ids.index(ac_id)] i2p = 1. / 2**8 # integer to position @@ -153,6 +159,23 @@ class Base: msg['heading'] = self.heading self._interface.send(msg) + msg2 = PprzMessage("ground", "FLIGHT_PARAM") + msg2['ac_id'] = "GCS" + msg2['roll'] = 0.0 + msg2['pitch'] = 0.0 + msg2['heading'] = self.heading + msg2['lat'] = self.lat + msg2['long'] = self.lon + msg2['speed'] = self.speed + msg2['course'] = self.course + msg2['alt'] = 0.0 + msg2['climb'] = 0.0 + msg2['agl'] = 0.0 + msg2['unix_time'] = 0.0 + msg2['itow'] = 0 + msg2['airspeed'] = self.speed + self._interface.send(msg2) + def run(self): try: # The main loop @@ -165,6 +188,9 @@ class Base: # Send base position if self.enabled: + #integrate derivatives + self.heading += self.step*self.turn_rate + self.course = self.heading dn = self.speed*m.cos(self.course/180.0*m.pi) de = self.speed*m.sin(self.course/180.0*m.pi) self.move_base(self.step*dn,self.step*de) @@ -177,7 +203,13 @@ class Base: if __name__ == '__main__': import argparse + parser = argparse.ArgumentParser(description="Moving base simulator") + parser.add_argument('-s', '--speed', dest='speed', default=1, type=float, help="Speed of the ship (m/s).") + parser.add_argument('-he', '--heading', dest='heading', default=0, type=float, help="Heading of the ship (deg).") + parser.add_argument('-t', '--turn_rate', dest='turn_rate', default=0, type=float, help="Turn rate of the ship (deg/s).") + parser.add_argument('-i', '--id', dest='id', default=0, type=float, help="Aircraft ID.") + args = parser.parse_args() - base = Base() + base = Base(speed=args.speed, heading=args.heading, turn_rate=args.turn_rate, id=args.id) base.run()