mirror of https://github.com/ArduPilot/ardupilot
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
This commit is contained in:
parent
8c84fa5c64
commit
4db011f530
|
@ -781,3 +781,24 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
|
||||||
precland->update(get_location(), get_position());
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
@ -243,6 +243,8 @@ protected:
|
||||||
// update external payload/sensor dynamic
|
// update external payload/sensor dynamic
|
||||||
void update_external_payload(const struct sitl_input &input);
|
void update_external_payload(const struct sitl_input &input);
|
||||||
|
|
||||||
|
void add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
uint64_t last_time_us = 0;
|
uint64_t last_time_us = 0;
|
||||||
uint32_t frame_counter = 0;
|
uint32_t frame_counter = 0;
|
||||||
|
|
|
@ -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)
|
void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
|
||||||
{
|
{
|
||||||
frame->calculate_forces(*this, input, rot_accel, body_accel);
|
frame->calculate_forces(*this, input, rot_accel, body_accel);
|
||||||
|
add_shove_forces(rot_accel, body_accel);
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
|
@ -148,6 +148,10 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
|
||||||
// @Path: ./SIM_Precland.cpp
|
// @Path: ./SIM_Precland.cpp
|
||||||
AP_SUBGROUPINFO(precland_sim, "PLD_", 29, SITL, SIM_Precland),
|
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
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -229,7 +229,16 @@ public:
|
||||||
|
|
||||||
// vibration frequencies in Hz on each axis
|
// vibration frequencies in Hz on each axis
|
||||||
AP_Vector3f vibe_freq;
|
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;
|
uint16_t irlock_port;
|
||||||
|
|
||||||
void simstate_send(mavlink_channel_t chan);
|
void simstate_send(mavlink_channel_t chan);
|
||||||
|
|
Loading…
Reference in New Issue