Browse Source

SITL: add SIM_SHOVE_* options to shove the simulated vehicle

The parameter values are time in milliseconds and body-frame
acceleration in m/s/s

So to test throw mode:
 mode throw
 param set SIM_SHOVE_Z -30
 arm throttle
 param set SIM_SHOVE_TIME 500
mission-4.1.18
Peter Barker 7 years ago committed by Peter Barker
parent
commit
4db011f530
  1. 21
      libraries/SITL/SIM_Aircraft.cpp
  2. 2
      libraries/SITL/SIM_Aircraft.h
  3. 1
      libraries/SITL/SIM_Multicopter.cpp
  4. 4
      libraries/SITL/SITL.cpp
  5. 11
      libraries/SITL/SITL.h

21
libraries/SITL/SIM_Aircraft.cpp

@ -781,3 +781,24 @@ void Aircraft::update_external_payload(const struct sitl_input &input) @@ -781,3 +781,24 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
precland->update(get_location(), get_position());
}
}
void Aircraft::add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel)
{
const uint32_t now = AP_HAL::millis();
if (sitl->shove.t == 0) {
return;
}
if (sitl->shove.start_ms == 0) {
sitl->shove.start_ms = now;
}
if (now - sitl->shove.start_ms < uint32_t(sitl->shove.t)) {
// FIXME: can we get a vector operation here instead?
body_accel.x += sitl->shove.x;
body_accel.y += sitl->shove.y;
body_accel.z += sitl->shove.z;
} else {
sitl->shove.start_ms = 0;
sitl->shove.t = 0;
}
}

2
libraries/SITL/SIM_Aircraft.h

@ -243,6 +243,8 @@ protected: @@ -243,6 +243,8 @@ protected:
// update external payload/sensor dynamic
void update_external_payload(const struct sitl_input &input);
void add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel);
private:
uint64_t last_time_us = 0;
uint32_t frame_counter = 0;

1
libraries/SITL/SIM_Multicopter.cpp

@ -49,6 +49,7 @@ MultiCopter::MultiCopter(const char *home_str, const char *frame_str) : @@ -49,6 +49,7 @@ MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
{
frame->calculate_forces(*this, input, rot_accel, body_accel);
add_shove_forces(rot_accel, body_accel);
}
/*

4
libraries/SITL/SITL.cpp

@ -148,6 +148,10 @@ const AP_Param::GroupInfo SITL::var_info2[] = { @@ -148,6 +148,10 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
// @Path: ./SIM_Precland.cpp
AP_SUBGROUPINFO(precland_sim, "PLD_", 29, SITL, SIM_Precland),
AP_GROUPINFO("SHOVE_X", 30, SITL, shove.x, 0),
AP_GROUPINFO("SHOVE_Y", 31, SITL, shove.y, 0),
AP_GROUPINFO("SHOVE_Z", 32, SITL, shove.z, 0),
AP_GROUPINFO("SHOVE_TIME", 33, SITL, shove.t, 0),
AP_GROUPEND
};

11
libraries/SITL/SITL.h

@ -229,7 +229,16 @@ public: @@ -229,7 +229,16 @@ public:
// vibration frequencies in Hz on each axis
AP_Vector3f vibe_freq;
struct {
AP_Float x;
AP_Float y;
AP_Float z;
AP_Int32 t;
uint32_t start_ms;
} shove;
uint16_t irlock_port;
void simstate_send(mavlink_channel_t chan);

Loading…
Cancel
Save