static void simulateModelAgnostic()
{
float Rbe[3][3];
float q[4];
// Simulate accels based on current attitude
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
Quaternion2R(q,Rbe);
AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = -GRAVITY * Rbe[0][2];
accelsData.y = -GRAVITY * Rbe[1][2];
accelsData.z = -GRAVITY * Rbe[2][2];
accelsData.temperature = 30;
AccelsSet(&accelsData);
RateDesiredData rateDesired;
RateDesiredGet(&rateDesired);
GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = rateDesired.Roll + rand_gauss();
gyrosData.y = rateDesired.Pitch + rand_gauss();
gyrosData.z = rateDesired.Yaw + rand_gauss();
// Apply bias correction to the gyros
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosData.x += gyrosBias.x;
gyrosData.y += gyrosBias.y;
gyrosData.z += gyrosBias.z;
GyrosSet(&gyrosData);
BaroAltitudeData baroAltitude;
BaroAltitudeGet(&baroAltitude);
baroAltitude.Altitude = 1;
BaroAltitudeSet(&baroAltitude);
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
gpsPosition.Latitude = 0;
gpsPosition.Longitude = 0;
gpsPosition.Altitude = 0;
GPSPositionSet(&gpsPosition);
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
MagnetometerData mag;
mag.x = 400;
mag.y = 0;
mag.z = 800;
MagnetometerSet(&mag);
}
void ch_awgn_c(const cf_t* x, cf_t* y, float variance, uint32_t len) {
cf_t tmp;
uint32_t i;
for (i=0;i<len;i++) {
__real__ tmp = rand_gauss();
__imag__ tmp = rand_gauss();
tmp *= variance;
y[i] = tmp + x[i];
}
}
void ch_awgn_c(const cf_t* x, cf_t* y, float variance, int buff_sz) {
_Complex float tmp;
int i;
for (i=0;i<buff_sz;i++) {
__real__ tmp = rand_gauss();
__imag__ tmp = rand_gauss();
tmp *= variance;
y[i] = tmp + x[i];
}
}
void ch_awgn_f(const float* x, float* y, float variance, uint32_t len) {
uint32_t i;
for (i=0;i<len;i++) {
y[i] = x[i] + variance * rand_gauss();
}
}
void graphene::generate(double sigma)
{
for(int i=0; i<TOT; i++)
{
p[i]=sigma*rand_gauss();
}
}
void ch_awgn_f(const float* x, float* y, float variance, int buff_sz) {
int i;
for (i=0;i<buff_sz;i++) {
y[i] = x[i] + variance * rand_gauss();
}
}
/**
* Initialise the module. Called before the start function
* \returns 0 on success or -1 if initialisation failed
*/
int32_t SensorsInitialize(void)
{
accel_bias[0] = rand_gauss() / 10;
accel_bias[1] = rand_gauss() / 10;
accel_bias[2] = rand_gauss() / 10;
AccelsInitialize();
AttitudeSimulatedInitialize();
BaroAltitudeInitialize();
BaroAirspeedInitialize();
GyrosInitialize();
GyrosBiasInitialize();
GPSPositionInitialize();
GPSVelocityInitialize();
MagnetometerInitialize();
MagBiasInitialize();
return 0;
}
// Randomize prefix C
static void repad_random_prefix(prefix_addr_t *address) {
uint32_t i;
uint32_t auxVal;
double possibilities = pow(REPA_FIELD_LENGTH, 2);
double mean = possibilities/2;
double variance = possibilities/2;
*address = 0;
for(i = 0; i < REPA_PREFIX_LENGTH; i++) {
auxVal = (int)trunc(rand_gauss(mean, variance));
*address |= (auxVal & REPA_MASK_FIELD(8*sizeof(*address),
REPA_PREFIX_LENGTH,0,REPA_FIELD_LENGTH)) << (i*(8*sizeof(*address)/REPA_PREFIX_LENGTH));
}
}
void rand_mgauss(int n, float * store){
for(int i=0; i < n; i++){
store[i] = rand_gauss();
}
}
// This function:
// - is called once for each motion event
// - does motion event interpolation
// - paints zero, one or several dabs
// - decides whether the stroke is finished (for undo/redo)
// returns true if the stroke is finished or empty
bool stroke_to (Surface * surface, float x, float y, float pressure, float xtilt, float ytilt, double dtime)
{
//printf("%f %f %f %f\n", (double)dtime, (double)x, (double)y, (double)pressure);
float tilt_ascension = 0.0;
float tilt_declination = 90.0;
if (xtilt != 0 || ytilt != 0) {
// shield us from insane tilt input
xtilt = CLAMP(xtilt, -1.0, 1.0);
ytilt = CLAMP(ytilt, -1.0, 1.0);
assert(std::isfinite(xtilt) && std::isfinite(ytilt));
tilt_ascension = 180.0*atan2(-xtilt, ytilt)/M_PI;
float e;
if (abs(xtilt) > abs(ytilt)) {
e = sqrt(1+ytilt*ytilt);
} else {
e = sqrt(1+xtilt*xtilt);
}
float rad = hypot(xtilt, ytilt);
float cos_alpha = rad/e;
if (cos_alpha >= 1.0) cos_alpha = 1.0; // fix numerical inaccuracy
tilt_declination = 180.0*acos(cos_alpha)/M_PI;
assert(std::isfinite(tilt_ascension));
assert(std::isfinite(tilt_declination));
}
// printf("xtilt %f, ytilt %f\n", (double)xtilt, (double)ytilt);
// printf("ascension %f, declination %f\n", (double)tilt_ascension, (double)tilt_declination);
pressure = CLAMP(pressure, 0.0, 1.0);
if (!std::isfinite(x) || !std::isfinite(y) ||
(x > 1e10 || y > 1e10 || x < -1e10 || y < -1e10)) {
// workaround attempt for https://gna.org/bugs/?14372
g_print("Warning: ignoring brush::stroke_to with insane inputs (x = %f, y = %f)\n", (double)x, (double)y);
x = 0.0;
y = 0.0;
pressure = 0.0;
}
// the assertion below is better than out-of-memory later at save time
assert(x < 1e8 && y < 1e8 && x > -1e8 && y > -1e8);
if (dtime < 0) g_print("Time jumped backwards by dtime=%f seconds!\n", dtime);
if (dtime <= 0) dtime = 0.0001; // protect against possible division by zero bugs
if (dtime > 0.100 && pressure && states[STATE_PRESSURE] == 0) {
// Workaround for tablets that don't report motion events without pressure.
// This is to avoid linear interpolation of the pressure between two events.
stroke_to (surface, x, y, 0.0, 90.0, 0.0, dtime-0.0001);
dtime = 0.0001;
}
g_rand_set_seed (rng, states[STATE_RNG_SEED]);
{ // calculate the actual "virtual" cursor position
// noise first
if (settings[BRUSH_TRACKING_NOISE]->base_value) {
// OPTIMIZE: expf() called too often
float base_radius = expf(settings[BRUSH_RADIUS_LOGARITHMIC]->base_value);
x += rand_gauss (rng) * settings[BRUSH_TRACKING_NOISE]->base_value * base_radius;
y += rand_gauss (rng) * settings[BRUSH_TRACKING_NOISE]->base_value * base_radius;
}
float fac = 1.0 - exp_decay (settings[BRUSH_SLOW_TRACKING]->base_value, 100.0*dtime);
x = states[STATE_X] + (x - states[STATE_X]) * fac;
y = states[STATE_Y] + (y - states[STATE_Y]) * fac;
}
// draw many (or zero) dabs to the next position
// see doc/stroke2dabs.png
float dist_moved = states[STATE_DIST];
float dist_todo = count_dabs_to (x, y, pressure, dtime);
//if (dtime > 5 || dist_todo > 300) {
if (dtime > 5 || reset_requested) {
reset_requested = false;
/*
TODO:
if (dist_todo > 300) {
// this happens quite often, eg when moving the cursor back into the window
// FIXME: bad to hardcode a distance treshold here - might look at zoomed image
// better detect leaving/entering the window and reset then.
g_print ("Warning: NOT drawing %f dabs.\n", dist_todo);
g_print ("dtime=%f, dx=%f\n", dtime, x-states[STATE_X]);
//must_reset = 1;
}
*/
//printf("Brush reset.\n");
for (int i=0; i<STATE_COUNT; i++) {
states[i] = 0;
}
states[STATE_X] = x;
states[STATE_Y] = y;
states[STATE_PRESSURE] = pressure;
// not resetting, because they will get overwritten below:
//dx, dy, dpress, dtime
states[STATE_ACTUAL_X] = states[STATE_X];
states[STATE_ACTUAL_Y] = states[STATE_Y];
states[STATE_STROKE] = 1.0; // start in a state as if the stroke was long finished
return true;
}
//g_print("dist = %f\n", states[STATE_DIST]);
enum { UNKNOWN, YES, NO } painted = UNKNOWN;
double dtime_left = dtime;
float step_dx, step_dy, step_dpressure, step_dtime;
float step_declination, step_ascension;
while (dist_moved + dist_todo >= 1.0) { // there are dabs pending
{ // linear interpolation (nonlinear variant was too slow, see SVN log)
float frac; // fraction of the remaining distance to move
if (dist_moved > 0) {
// "move" the brush exactly to the first dab (moving less than one dab)
frac = (1.0 - dist_moved) / dist_todo;
dist_moved = 0;
} else {
// "move" the brush from one dab to the next
frac = 1.0 / dist_todo;
}
step_dx = frac * (x - states[STATE_X]);
step_dy = frac * (y - states[STATE_Y]);
step_dpressure = frac * (pressure - states[STATE_PRESSURE]);
step_dtime = frac * (dtime_left - 0.0);
step_declination = frac * (tilt_declination - states[STATE_DECLINATION]);
step_ascension = frac * (tilt_ascension - states[STATE_ASCENSION]);
// Though it looks different, time is interpolated exactly like x/y/pressure.
}
update_states_and_setting_values (step_dx, step_dy, step_dpressure, step_declination, step_ascension, step_dtime);
bool painted_now = prepare_and_draw_dab (surface);
if (painted_now) {
painted = YES;
} else if (painted == UNKNOWN) {
painted = NO;
}
dtime_left -= step_dtime;
dist_todo = count_dabs_to (x, y, pressure, dtime_left);
}
{
// "move" the brush to the current time (no more dab will happen)
// Important to do this at least once every event, because
// brush_count_dabs_to depends on the radius and the radius can
// depend on something that changes much faster than only every
// dab (eg speed).
step_dx = x - states[STATE_X];
step_dy = y - states[STATE_Y];
step_dpressure = pressure - states[STATE_PRESSURE];
step_declination = tilt_declination - states[STATE_DECLINATION];
step_ascension = tilt_ascension - states[STATE_ASCENSION];
step_dtime = dtime_left;
//dtime_left = 0; but that value is not used any more
update_states_and_setting_values (step_dx, step_dy, step_dpressure, step_declination, step_ascension, step_dtime);
}
// save the fraction of a dab that is already done now
states[STATE_DIST] = dist_moved + dist_todo;
//g_print("dist_final = %f\n", states[STATE_DIST]);
// next seed for the RNG (GRand has no get_state() and states[] must always contain our full state)
states[STATE_RNG_SEED] = g_rand_int(rng);
// stroke separation logic (for undo/redo)
if (painted == UNKNOWN) {
if (stroke_current_idling_time > 0 || stroke_total_painting_time == 0) {
// still idling
painted = NO;
} else {
// probably still painting (we get more events than brushdabs)
painted = YES;
//if (pressure == 0) g_print ("info: assuming 'still painting' while there is no pressure\n");
}
}
if (painted == YES) {
//if (stroke_current_idling_time > 0) g_print ("idling ==> painting\n");
stroke_total_painting_time += dtime;
stroke_current_idling_time = 0;
// force a stroke split after some time
if (stroke_total_painting_time > 4 + 3*pressure) {
// but only if pressure is not being released
// FIXME: use some smoothed state for dpressure, not the output of the interpolation code
// (which might easily wrongly give dpressure == 0)
if (step_dpressure >= 0) {
return true;
}
}
} else if (painted == NO) {
//if (stroke_current_idling_time == 0) g_print ("painting ==> idling\n");
stroke_current_idling_time += dtime;
if (stroke_total_painting_time == 0) {
// not yet painted, start a new stroke if we have accumulated a lot of irrelevant motion events
if (stroke_current_idling_time > 1.0) {
return true;
}
} else {
// Usually we have pressure==0 here. But some brushes can paint
// nothing at full pressure (eg gappy lines, or a stroke that
// fades out). In either case this is the prefered moment to split.
if (stroke_total_painting_time+stroke_current_idling_time > 0.9 + 5*pressure) {
return true;
}
}
}
return false;
}
// Called only from stroke_to(). Calculate everything needed to
// draw the dab, then let the surface do the actual drawing.
//
// This is only gets called right after update_states_and_setting_values().
// Returns true if the surface was modified.
bool prepare_and_draw_dab (Surface * surface)
{
float x, y, opaque;
float radius;
// ensure we don't get a positive result with two negative opaque values
if (settings_value[BRUSH_OPAQUE] < 0) settings_value[BRUSH_OPAQUE] = 0;
opaque = settings_value[BRUSH_OPAQUE] * settings_value[BRUSH_OPAQUE_MULTIPLY];
opaque = CLAMP(opaque, 0.0, 1.0);
//if (opaque == 0.0) return false; <-- cannot do that, since we need to update smudge state.
if (settings_value[BRUSH_OPAQUE_LINEARIZE]) {
// OPTIMIZE: no need to recalculate this for each dab
float alpha, beta, alpha_dab, beta_dab;
float dabs_per_pixel;
// dabs_per_pixel is just estimated roughly, I didn't think hard
// about the case when the radius changes during the stroke
dabs_per_pixel = (
settings[BRUSH_DABS_PER_ACTUAL_RADIUS]->base_value +
settings[BRUSH_DABS_PER_BASIC_RADIUS]->base_value
) * 2.0;
// the correction is probably not wanted if the dabs don't overlap
if (dabs_per_pixel < 1.0) dabs_per_pixel = 1.0;
// interpret the user-setting smoothly
dabs_per_pixel = 1.0 + settings[BRUSH_OPAQUE_LINEARIZE]->base_value*(dabs_per_pixel-1.0);
// see doc/brushdab_saturation.png
// beta = beta_dab^dabs_per_pixel
// <==> beta_dab = beta^(1/dabs_per_pixel)
alpha = opaque;
beta = 1.0-alpha;
beta_dab = powf(beta, 1.0/dabs_per_pixel);
alpha_dab = 1.0-beta_dab;
opaque = alpha_dab;
}
x = states[STATE_ACTUAL_X];
y = states[STATE_ACTUAL_Y];
float base_radius = expf(settings[BRUSH_RADIUS_LOGARITHMIC]->base_value);
if (settings_value[BRUSH_OFFSET_BY_SPEED]) {
x += states[STATE_NORM_DX_SLOW] * settings_value[BRUSH_OFFSET_BY_SPEED] * 0.1 * base_radius;
y += states[STATE_NORM_DY_SLOW] * settings_value[BRUSH_OFFSET_BY_SPEED] * 0.1 * base_radius;
}
if (settings_value[BRUSH_OFFSET_BY_RANDOM]) {
float amp = settings_value[BRUSH_OFFSET_BY_RANDOM];
if (amp < 0.0) amp = 0.0;
x += rand_gauss (rng) * amp * base_radius;
y += rand_gauss (rng) * amp * base_radius;
}
radius = states[STATE_ACTUAL_RADIUS];
if (settings_value[BRUSH_RADIUS_BY_RANDOM]) {
float radius_log, alpha_correction;
// go back to logarithmic radius to add the noise
radius_log = settings_value[BRUSH_RADIUS_LOGARITHMIC];
radius_log += rand_gauss (rng) * settings_value[BRUSH_RADIUS_BY_RANDOM];
radius = expf(radius_log);
radius = CLAMP(radius, ACTUAL_RADIUS_MIN, ACTUAL_RADIUS_MAX);
alpha_correction = states[STATE_ACTUAL_RADIUS] / radius;
alpha_correction = SQR(alpha_correction);
if (alpha_correction <= 1.0) {
opaque *= alpha_correction;
}
}
// color part
float color_h = settings[BRUSH_COLOR_H]->base_value;
float color_s = settings[BRUSH_COLOR_S]->base_value;
float color_v = settings[BRUSH_COLOR_V]->base_value;
float eraser_target_alpha = 1.0;
if (settings_value[BRUSH_SMUDGE] > 0.0) {
// mix (in RGB) the smudge color with the brush color
hsv_to_rgb_float (&color_h, &color_s, &color_v);
float fac = settings_value[BRUSH_SMUDGE];
if (fac > 1.0) fac = 1.0;
// If the smudge color somewhat transparent, then the resulting
// dab will do erasing towards that transparency level.
// see also ../doc/smudge_math.png
eraser_target_alpha = (1-fac)*1.0 + fac*states[STATE_SMUDGE_A];
// fix rounding errors (they really seem to happen in the previous line)
eraser_target_alpha = CLAMP(eraser_target_alpha, 0.0, 1.0);
if (eraser_target_alpha > 0) {
color_h = (fac*states[STATE_SMUDGE_RA] + (1-fac)*color_h) / eraser_target_alpha;
color_s = (fac*states[STATE_SMUDGE_GA] + (1-fac)*color_s) / eraser_target_alpha;
color_v = (fac*states[STATE_SMUDGE_BA] + (1-fac)*color_v) / eraser_target_alpha;
} else {
// we are only erasing; the color does not matter
color_h = 1.0;
color_s = 0.0;
color_v = 0.0;
}
rgb_to_hsv_float (&color_h, &color_s, &color_v);
}
if (settings_value[BRUSH_SMUDGE_LENGTH] < 1.0 and
// optimization, since normal brushes have smudge_length == 0.5 without actually smudging
(settings_value[BRUSH_SMUDGE] != 0.0 or not settings[BRUSH_SMUDGE]->is_constant())) {
float fac = settings_value[BRUSH_SMUDGE_LENGTH];
if (fac < 0.01) fac = 0.01;
int px, py;
px = ROUND(x);
py = ROUND(y);
// Calling get_color() is almost as expensive as rendering a
// dab. Because of this we use the previous value if it is not
// expected to hurt quality too much. We call it at most every
// second dab.
float r, g, b, a;
states[STATE_LAST_GETCOLOR_RECENTNESS] *= fac;
if (states[STATE_LAST_GETCOLOR_RECENTNESS] < 0.5*fac) {
states[STATE_LAST_GETCOLOR_RECENTNESS] = 1.0;
float smudge_radius = radius * expf(settings_value[BRUSH_SMUDGE_RADIUS_LOG]);
smudge_radius = CLAMP(smudge_radius, ACTUAL_RADIUS_MIN, ACTUAL_RADIUS_MAX);
surface->get_color (px, py, smudge_radius, &r, &g, &b, &a);
states[STATE_LAST_GETCOLOR_R] = r;
states[STATE_LAST_GETCOLOR_G] = g;
states[STATE_LAST_GETCOLOR_B] = b;
states[STATE_LAST_GETCOLOR_A] = a;
} else {
r = states[STATE_LAST_GETCOLOR_R];
g = states[STATE_LAST_GETCOLOR_G];
b = states[STATE_LAST_GETCOLOR_B];
a = states[STATE_LAST_GETCOLOR_A];
}
// updated the smudge color (stored with premultiplied alpha)
states[STATE_SMUDGE_A ] = fac*states[STATE_SMUDGE_A ] + (1-fac)*a;
// fix rounding errors
states[STATE_SMUDGE_A ] = CLAMP(states[STATE_SMUDGE_A], 0.0, 1.0);
states[STATE_SMUDGE_RA] = fac*states[STATE_SMUDGE_RA] + (1-fac)*r*a;
states[STATE_SMUDGE_GA] = fac*states[STATE_SMUDGE_GA] + (1-fac)*g*a;
states[STATE_SMUDGE_BA] = fac*states[STATE_SMUDGE_BA] + (1-fac)*b*a;
}
// eraser
if (settings_value[BRUSH_ERASER]) {
eraser_target_alpha *= (1.0-settings_value[BRUSH_ERASER]);
}
// HSV color change
color_h += settings_value[BRUSH_CHANGE_COLOR_H];
color_s += settings_value[BRUSH_CHANGE_COLOR_HSV_S];
color_v += settings_value[BRUSH_CHANGE_COLOR_V];
// HSL color change
if (settings_value[BRUSH_CHANGE_COLOR_L] || settings_value[BRUSH_CHANGE_COLOR_HSL_S]) {
// (calculating way too much here, can be optimized if neccessary)
// this function will CLAMP the inputs
hsv_to_rgb_float (&color_h, &color_s, &color_v);
rgb_to_hsl_float (&color_h, &color_s, &color_v);
color_v += settings_value[BRUSH_CHANGE_COLOR_L];
color_s += settings_value[BRUSH_CHANGE_COLOR_HSL_S];
hsl_to_rgb_float (&color_h, &color_s, &color_v);
rgb_to_hsv_float (&color_h, &color_s, &color_v);
}
float hardness = CLAMP(settings_value[BRUSH_HARDNESS], 0.0, 1.0);
// anti-aliasing attempt (works surprisingly well for ink brushes)
float current_fadeout_in_pixels = radius * (1.0 - hardness);
float min_fadeout_in_pixels = settings_value[BRUSH_ANTI_ALIASING];
if (current_fadeout_in_pixels < min_fadeout_in_pixels) {
// need to soften the brush (decrease hardness), but keep optical radius
// so we tune both radius and hardness, to get the desired fadeout_in_pixels
float current_optical_radius = radius - (1.0-hardness)*radius/2.0;
// Equation 1: (new fadeout must be equal to min_fadeout)
// min_fadeout_in_pixels = radius_new*(1.0 - hardness_new)
// Equation 2: (optical radius must remain unchanged)
// current_optical_radius = radius_new - (1.0-hardness_new)*radius_new/2.0
//
// Solved Equation 1 for hardness_new, using Equation 2: (thanks to mathomatic)
float hardness_new = ((current_optical_radius - (min_fadeout_in_pixels/2.0))/(current_optical_radius + (min_fadeout_in_pixels/2.0)));
// Using Equation 1:
float radius_new = (min_fadeout_in_pixels/(1.0 - hardness_new));
hardness = hardness_new;
radius = radius_new;
}
// the functions below will CLAMP most inputs
hsv_to_rgb_float (&color_h, &color_s, &color_v);
return surface->draw_dab (x, y, radius, color_h, color_s, color_v, opaque, hardness, eraser_target_alpha,
states[STATE_ACTUAL_ELLIPTICAL_DAB_RATIO], states[STATE_ACTUAL_ELLIPTICAL_DAB_ANGLE],
settings_value[BRUSH_LOCK_ALPHA]);
}
// handles bang from inlet
void bangGenerateNum(t_gauss *x)
{
t_float num = rand_gauss(x->mean, x->stddev);
outlet_float(x->x_obj.ob_outlet, num);
}
/**
* mypaint_brush_stroke_to:
* @dtime: Time since last motion event, in seconds.
*
* Should be called once for each motion event.
*
* Returns: non-0 if the stroke is finished or empty, else 0.
*/
int mypaint_brush_stroke_to (MyPaintBrush *self, MyPaintSurface *surface,
float x, float y, float pressure,
float xtilt, float ytilt, double dtime)
{
//printf("%f %f %f %f\n", (double)dtime, (double)x, (double)y, (double)pressure);
float tilt_ascension = 0.0;
float tilt_declination = 90.0;
if (xtilt != 0 || ytilt != 0) {
// shield us from insane tilt input
xtilt = CLAMP(xtilt, -1.0, 1.0);
ytilt = CLAMP(ytilt, -1.0, 1.0);
assert(isfinite(xtilt) && isfinite(ytilt));
tilt_ascension = 180.0*atan2(-xtilt, ytilt)/M_PI;
const float rad = hypot(xtilt, ytilt);
tilt_declination = 90-(rad*60);
assert(isfinite(tilt_ascension));
assert(isfinite(tilt_declination));
}
// printf("xtilt %f, ytilt %f\n", (double)xtilt, (double)ytilt);
// printf("ascension %f, declination %f\n", (double)tilt_ascension, (double)tilt_declination);
if (pressure <= 0.0) pressure = 0.0;
if (!isfinite(x) || !isfinite(y) ||
(x > 1e10 || y > 1e10 || x < -1e10 || y < -1e10)) {
// workaround attempt for https://gna.org/bugs/?14372
printf("Warning: ignoring brush::stroke_to with insane inputs (x = %f, y = %f)\n", (double)x, (double)y);
x = 0.0;
y = 0.0;
pressure = 0.0;
}
// the assertion below is better than out-of-memory later at save time
assert(x < 1e8 && y < 1e8 && x > -1e8 && y > -1e8);
if (dtime < 0) printf("Time jumped backwards by dtime=%f seconds!\n", dtime);
if (dtime <= 0) dtime = 0.0001; // protect against possible division by zero bugs
/* way too slow with the new rng, and not working any more anyway...
rng_double_set_seed (self->rng, self->states[MYPAINT_BRUSH_STATE_RNG_SEED]*0x40000000);
*/
if (dtime > 0.100 && pressure && self->states[MYPAINT_BRUSH_STATE_PRESSURE] == 0) {
// Workaround for tablets that don't report motion events without pressure.
// This is to avoid linear interpolation of the pressure between two events.
mypaint_brush_stroke_to (self, surface, x, y, 0.0, 90.0, 0.0, dtime-0.0001);
dtime = 0.0001;
}
{ // calculate the actual "virtual" cursor position
// noise first
if (mypaint_mapping_get_base_value(self->settings[MYPAINT_BRUSH_SETTING_TRACKING_NOISE])) {
// OPTIMIZE: expf() called too often
const float base_radius = expf(mypaint_mapping_get_base_value(self->settings[MYPAINT_BRUSH_SETTING_RADIUS_LOGARITHMIC]));
x += rand_gauss (self->rng) * mypaint_mapping_get_base_value(self->settings[MYPAINT_BRUSH_SETTING_TRACKING_NOISE]) * base_radius;
y += rand_gauss (self->rng) * mypaint_mapping_get_base_value(self->settings[MYPAINT_BRUSH_SETTING_TRACKING_NOISE]) * base_radius;
}
const float fac = 1.0 - exp_decay (mypaint_mapping_get_base_value(self->settings[MYPAINT_BRUSH_SETTING_SLOW_TRACKING]), 100.0*dtime);
x = self->states[MYPAINT_BRUSH_STATE_X] + (x - self->states[MYPAINT_BRUSH_STATE_X]) * fac;
y = self->states[MYPAINT_BRUSH_STATE_Y] + (y - self->states[MYPAINT_BRUSH_STATE_Y]) * fac;
}
// draw many (or zero) dabs to the next position
// see doc/images/stroke2dabs.png
float dabs_moved = self->states[MYPAINT_BRUSH_STATE_PARTIAL_DABS];
float dabs_todo = count_dabs_to (self, x, y, pressure, dtime);
if (dtime > 5 || self->reset_requested) {
self->reset_requested = FALSE;
//printf("Brush reset.\n");
int i=0;
for (i=0; i<MYPAINT_BRUSH_STATES_COUNT; i++) {
self->states[i] = 0;
}
self->states[MYPAINT_BRUSH_STATE_X] = x;
self->states[MYPAINT_BRUSH_STATE_Y] = y;
self->states[MYPAINT_BRUSH_STATE_PRESSURE] = pressure;
// not resetting, because they will get overwritten below:
//dx, dy, dpress, dtime
self->states[MYPAINT_BRUSH_STATE_ACTUAL_X] = self->states[MYPAINT_BRUSH_STATE_X];
self->states[MYPAINT_BRUSH_STATE_ACTUAL_Y] = self->states[MYPAINT_BRUSH_STATE_Y];
self->states[MYPAINT_BRUSH_STATE_STROKE] = 1.0; // start in a state as if the stroke was long finished
return TRUE;
}
enum { UNKNOWN, YES, NO } painted = UNKNOWN;
double dtime_left = dtime;
float step_ddab, step_dx, step_dy, step_dpressure, step_dtime;
float step_declination, step_ascension;
while (dabs_moved + dabs_todo >= 1.0) { // there are dabs pending
{ // linear interpolation (nonlinear variant was too slow, see SVN log)
float frac; // fraction of the remaining distance to move
if (dabs_moved > 0) {
// "move" the brush exactly to the first dab
step_ddab = 1.0 - dabs_moved; // the step "moves" the brush by a fraction of one dab
dabs_moved = 0;
} else {
step_ddab = 1.0; // the step "moves" the brush by exactly one dab
}
frac = step_ddab / dabs_todo;
step_dx = frac * (x - self->states[MYPAINT_BRUSH_STATE_X]);
step_dy = frac * (y - self->states[MYPAINT_BRUSH_STATE_Y]);
step_dpressure = frac * (pressure - self->states[MYPAINT_BRUSH_STATE_PRESSURE]);
step_dtime = frac * (dtime_left - 0.0);
// Though it looks different, time is interpolated exactly like x/y/pressure.
step_declination = frac * (tilt_declination - self->states[MYPAINT_BRUSH_STATE_DECLINATION]);
step_ascension = frac * smallest_angular_difference(self->states[MYPAINT_BRUSH_STATE_ASCENSION], tilt_ascension);
}
update_states_and_setting_values (self, step_ddab, step_dx, step_dy, step_dpressure, step_declination, step_ascension, step_dtime);
gboolean painted_now = prepare_and_draw_dab (self, surface);
if (painted_now) {
painted = YES;
} else if (painted == UNKNOWN) {
painted = NO;
}
dtime_left -= step_dtime;
dabs_todo = count_dabs_to (self, x, y, pressure, dtime_left);
}
{
// "move" the brush to the current time (no more dab will happen)
// Important to do this at least once every event, because
// brush_count_dabs_to depends on the radius and the radius can
// depend on something that changes much faster than just every
// dab.
step_ddab = dabs_todo; // the step "moves" the brush by a fraction of one dab
step_dx = x - self->states[MYPAINT_BRUSH_STATE_X];
step_dy = y - self->states[MYPAINT_BRUSH_STATE_Y];
step_dpressure = pressure - self->states[MYPAINT_BRUSH_STATE_PRESSURE];
step_declination = tilt_declination - self->states[MYPAINT_BRUSH_STATE_DECLINATION];
step_ascension = smallest_angular_difference(self->states[MYPAINT_BRUSH_STATE_ASCENSION], tilt_ascension);
step_dtime = dtime_left;
//dtime_left = 0; but that value is not used any more
update_states_and_setting_values (self, step_ddab, step_dx, step_dy, step_dpressure, step_declination, step_ascension, step_dtime);
}
// save the fraction of a dab that is already done now
self->states[MYPAINT_BRUSH_STATE_PARTIAL_DABS] = dabs_moved + dabs_todo;
/* not working any more with the new rng...
// next seed for the RNG (GRand has no get_state() and states[] must always contain our full state)
self->states[MYPAINT_BRUSH_STATE_RNG_SEED] = rng_double_next(self->rng);
*/
// stroke separation logic (for undo/redo)
if (painted == UNKNOWN) {
if (self->stroke_current_idling_time > 0 || self->stroke_total_painting_time == 0) {
// still idling
painted = NO;
} else {
// probably still painting (we get more events than brushdabs)
painted = YES;
//if (pressure == 0) g_print ("info: assuming 'still painting' while there is no pressure\n");
}
}
if (painted == YES) {
//if (stroke_current_idling_time > 0) g_print ("idling ==> painting\n");
self->stroke_total_painting_time += dtime;
self->stroke_current_idling_time = 0;
// force a stroke split after some time
if (self->stroke_total_painting_time > 4 + 3*pressure) {
// but only if pressure is not being released
// FIXME: use some smoothed state for dpressure, not the output of the interpolation code
// (which might easily wrongly give dpressure == 0)
if (step_dpressure >= 0) {
return TRUE;
}
}
} else if (painted == NO) {
//if (stroke_current_idling_time == 0) g_print ("painting ==> idling\n");
self->stroke_current_idling_time += dtime;
if (self->stroke_total_painting_time == 0) {
// not yet painted, start a new stroke if we have accumulated a lot of irrelevant motion events
if (self->stroke_current_idling_time > 1.0) {
return TRUE;
}
} else {
// Usually we have pressure==0 here. But some brushes can paint
// nothing at full pressure (eg gappy lines, or a stroke that
// fades out). In either case this is the prefered moment to split.
if (self->stroke_total_painting_time+self->stroke_current_idling_time > 0.9 + 5*pressure) {
return TRUE;
}
}
}
return FALSE;
}
// Called only from stroke_to(). Calculate everything needed to
// draw the dab, then let the surface do the actual drawing.
//
// This is only gets called right after update_states_and_setting_values().
// Returns true if the surface was modified.
bool prepare_and_draw_dab (Surface * surface)
{
float x, y, opaque;
float radius;
opaque = settings_value[BRUSH_OPAQUE] * settings_value[BRUSH_OPAQUE_MULTIPLY];
if (opaque >= 1.0) opaque = 1.0;
if (opaque <= 0.0) opaque = 0.0;
//if (opaque == 0.0) return false; <-- cannot do that, since we need to update smudge state.
if (settings_value[BRUSH_OPAQUE_LINEARIZE]) {
// OPTIMIZE: no need to recalculate this for each dab
float alpha, beta, alpha_dab, beta_dab;
float dabs_per_pixel;
// dabs_per_pixel is just estimated roughly, I didn't think hard
// about the case when the radius changes during the stroke
dabs_per_pixel = (
settings[BRUSH_DABS_PER_ACTUAL_RADIUS]->base_value +
settings[BRUSH_DABS_PER_BASIC_RADIUS]->base_value
) * 2.0;
// the correction is probably not wanted if the dabs don't overlap
if (dabs_per_pixel < 1.0) dabs_per_pixel = 1.0;
// interpret the user-setting smoothly
dabs_per_pixel = 1.0 + settings[BRUSH_OPAQUE_LINEARIZE]->base_value*(dabs_per_pixel-1.0);
// see doc/brushdab_saturation.png
// beta = beta_dab^dabs_per_pixel
// <==> beta_dab = beta^(1/dabs_per_pixel)
alpha = opaque;
beta = 1.0-alpha;
beta_dab = powf(beta, 1.0/dabs_per_pixel);
alpha_dab = 1.0-beta_dab;
opaque = alpha_dab;
}
x = states[STATE_ACTUAL_X];
y = states[STATE_ACTUAL_Y];
float base_radius = expf(settings[BRUSH_RADIUS_LOGARITHMIC]->base_value);
if (settings_value[BRUSH_OFFSET_BY_SPEED]) {
x += states[STATE_NORM_DX_SLOW] * settings_value[BRUSH_OFFSET_BY_SPEED] * 0.1 * base_radius;
y += states[STATE_NORM_DY_SLOW] * settings_value[BRUSH_OFFSET_BY_SPEED] * 0.1 * base_radius;
}
if (settings_value[BRUSH_OFFSET_BY_RANDOM]) {
x += rand_gauss (rng) * settings_value[BRUSH_OFFSET_BY_RANDOM] * base_radius;
y += rand_gauss (rng) * settings_value[BRUSH_OFFSET_BY_RANDOM] * base_radius;
}
radius = states[STATE_ACTUAL_RADIUS];
if (settings_value[BRUSH_RADIUS_BY_RANDOM]) {
float radius_log, alpha_correction;
// go back to logarithmic radius to add the noise
radius_log = settings_value[BRUSH_RADIUS_LOGARITHMIC];
radius_log += rand_gauss (rng) * settings_value[BRUSH_RADIUS_BY_RANDOM];
radius = expf(radius_log);
if (radius < ACTUAL_RADIUS_MIN) radius = ACTUAL_RADIUS_MIN;
if (radius > ACTUAL_RADIUS_MAX) radius = ACTUAL_RADIUS_MAX;
alpha_correction = states[STATE_ACTUAL_RADIUS] / radius;
alpha_correction = SQR(alpha_correction);
if (alpha_correction <= 1.0) {
opaque *= alpha_correction;
}
}
// color part
float color_h = settings[BRUSH_COLOR_H]->base_value;
float color_s = settings[BRUSH_COLOR_S]->base_value;
float color_v = settings[BRUSH_COLOR_V]->base_value;
float eraser_target_alpha = 1.0;
if (settings_value[BRUSH_SMUDGE] > 0.0) {
// mix (in RGB) the smudge color with the brush color
hsv_to_rgb_float (&color_h, &color_s, &color_v);
float fac = settings_value[BRUSH_SMUDGE];
if (fac > 1.0) fac = 1.0;
// If the smudge color somewhat transparent, then the resulting
// dab will do erasing towards that transparency level.
// see also ../doc/smudge_math.png
eraser_target_alpha = (1-fac)*1.0 + fac*states[STATE_SMUDGE_A];
// fix rounding errors (they really seem to happen in the previous line)
eraser_target_alpha = CLAMP(eraser_target_alpha, 0.0, 1.0);
if (eraser_target_alpha > 0) {
color_h = (fac*states[STATE_SMUDGE_RA] + (1-fac)*color_h) / eraser_target_alpha;
color_s = (fac*states[STATE_SMUDGE_GA] + (1-fac)*color_s) / eraser_target_alpha;
color_v = (fac*states[STATE_SMUDGE_BA] + (1-fac)*color_v) / eraser_target_alpha;
} else {
// we are only erasing; the color does not matter
color_h = 1.0;
color_s = 0.0;
color_v = 0.0;
}
rgb_to_hsv_float (&color_h, &color_s, &color_v);
}
if (settings_value[BRUSH_SMUDGE_LENGTH] < 1.0 &&
// optimization, since normal brushes have smudge_length == 0.5 without actually smudging
(settings_value[BRUSH_SMUDGE] != 0.0 || !settings[BRUSH_SMUDGE]->is_constant())) {
float fac = settings_value[BRUSH_SMUDGE_LENGTH];
if (fac < 0.0) fac = 0;
int px, py;
px = ROUND(x);
py = ROUND(y);
float r, g, b, a;
surface->get_color (px, py, radius, &r, &g, &b, &a);
// updated the smudge color (stored with premultiplied alpha)
states[STATE_SMUDGE_A ] = fac*states[STATE_SMUDGE_A ] + (1-fac)*a;
// fix rounding errors
states[STATE_SMUDGE_A ] = CLAMP(states[STATE_SMUDGE_A], 0.0, 1.0);
states[STATE_SMUDGE_RA] = fac*states[STATE_SMUDGE_RA] + (1-fac)*r*a;
states[STATE_SMUDGE_GA] = fac*states[STATE_SMUDGE_GA] + (1-fac)*g*a;
states[STATE_SMUDGE_BA] = fac*states[STATE_SMUDGE_BA] + (1-fac)*b*a;
}
// eraser
if (settings_value[BRUSH_ERASER]) {
eraser_target_alpha *= (1.0-settings_value[BRUSH_ERASER]);
}
// HSV color change
color_h += settings_value[BRUSH_CHANGE_COLOR_H];
color_s += settings_value[BRUSH_CHANGE_COLOR_HSV_S];
color_v += settings_value[BRUSH_CHANGE_COLOR_V];
// HSL color change
if (settings_value[BRUSH_CHANGE_COLOR_L] || settings_value[BRUSH_CHANGE_COLOR_HSL_S]) {
// (calculating way too much here, can be optimized if neccessary)
// this function will CLAMP the inputs
hsv_to_rgb_float (&color_h, &color_s, &color_v);
rgb_to_hsl_float (&color_h, &color_s, &color_v);
color_v += settings_value[BRUSH_CHANGE_COLOR_L];
color_s += settings_value[BRUSH_CHANGE_COLOR_HSL_S];
hsl_to_rgb_float (&color_h, &color_s, &color_v);
rgb_to_hsv_float (&color_h, &color_s, &color_v);
}
float hardness = settings_value[BRUSH_HARDNESS];
// the functions below will CLAMP most inputs
hsv_to_rgb_float (&color_h, &color_s, &color_v);
return surface->draw_dab (x, y, radius, color_h, color_s, color_v, opaque, hardness, eraser_target_alpha,
states[STATE_ACTUAL_ELLIPTICAL_DAB_RATIO], states[STATE_ACTUAL_ELLIPTICAL_DAB_ANGLE]);
}
/**
* This method performs a simple simulation of a car
*
* It takes in the ActuatorDesired command to rotate the aircraft and performs
* a simple kinetic model where the throttle increases the energy and drag decreases
* it. Changing altitude moves energy from kinetic to potential.
*
* 1. Update attitude based on ActuatorDesired
* 2. Update position based on velocity
*/
static void simulateModelCar()
{
static double pos[3] = {0,0,0};
static double vel[3] = {0,0,0};
static double ned_accel[3] = {0,0,0};
static float q[4] = {1,0,0,0};
static float rpy[3] = {0,0,0}; // Low pass filtered actuator
static float baro_offset = 0.0f;
float Rbe[3][3];
const float ACTUATOR_ALPHA = 0.8;
const float MAX_THRUST = 9.81 * 0.5;
const float K_FRICTION = 0.2;
const float GPS_PERIOD = 0.1;
const float MAG_PERIOD = 1.0 / 75.0;
const float BARO_PERIOD = 1.0 / 20.0;
static uint32_t last_time;
float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6);
if(dT < 1e-3)
dT = 2e-3;
last_time = PIOS_DELAY_GetRaw();
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
ActuatorDesiredData actuatorDesired;
ActuatorDesiredGet(&actuatorDesired);
float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0;
if (thrust < 0)
thrust = 0;
if (thrust != thrust)
thrust = 0;
// float control_scaling = thrust * thrustToDegs;
// // In rad/s
// rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
// rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
// rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
//
// GyrosData gyrosData; // Skip get as we set all the fields
// gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss();
// gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss();
// gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss();
/**** 1. Update attitude ****/
RateDesiredData rateDesired;
RateDesiredGet(&rateDesired);
// Need to get roll angle for easy cross coupling
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
rpy[0] = 0; // cannot roll
rpy[1] = 0; // cannot pitch
rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = rpy[0] + rand_gauss();
gyrosData.y = rpy[1] + rand_gauss();
gyrosData.z = rpy[2] + rand_gauss();
GyrosSet(&gyrosData);
// Predict the attitude forward in time
float qdot[4];
qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * DEG2RAD / 2;
qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * DEG2RAD / 2;
qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * DEG2RAD / 2;
qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * DEG2RAD / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
if(overideAttitude){
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
attitudeActual.q1 = q[0];
attitudeActual.q2 = q[1];
attitudeActual.q3 = q[2];
attitudeActual.q4 = q[3];
AttitudeActualSet(&attitudeActual);
}
/**** 2. Update position based on velocity ****/
// Rbe takes a vector from body to earth. If we take (1,0,0)^T through this and then dot with airspeed
// we get forward airspeed
Quaternion2R(q,Rbe);
double groundspeed[3] = {vel[0], vel[1], vel[2] };
double forwardSpeed = Rbe[0][0] * groundspeed[0] + Rbe[0][1] * groundspeed[1] + Rbe[0][2] * groundspeed[2];
double sidewaysSpeed = Rbe[1][0] * groundspeed[0] + Rbe[1][1] * groundspeed[1] + Rbe[1][2] * groundspeed[2];
/* Compute aerodynamic forces in body referenced frame. Later use more sophisticated equations */
/* TODO: This should become more accurate. Use the force equations to calculate lift from the */
/* various surfaces based on AoA and airspeed. From that compute torques and forces. For later */
double forces[3]; // X, Y, Z
forces[0] = thrust - forwardSpeed * K_FRICTION; // Friction is applied in all directions in NED
forces[1] = 0 - sidewaysSpeed * K_FRICTION * 100; // No side slip
forces[2] = 0;
// Negate force[2] as NED defines down as possitive, aircraft convention is Z up is positive (?)
ned_accel[0] = forces[0] * Rbe[0][0] + forces[1] * Rbe[1][0] - forces[2] * Rbe[2][0];
ned_accel[1] = forces[0] * Rbe[0][1] + forces[1] * Rbe[1][1] - forces[2] * Rbe[2][1];
ned_accel[2] = 0;
// Apply acceleration based on velocity
ned_accel[0] -= K_FRICTION * (vel[0]);
ned_accel[1] -= K_FRICTION * (vel[1]);
// Predict the velocity forward in time
vel[0] = vel[0] + ned_accel[0] * dT;
vel[1] = vel[1] + ned_accel[1] * dT;
vel[2] = vel[2] + ned_accel[2] * dT;
// Predict the position forward in time
pos[0] = pos[0] + vel[0] * dT;
pos[1] = pos[1] + vel[1] * dT;
pos[2] = pos[2] + vel[2] * dT;
// Simulate hitting ground
if(pos[2] > 0) {
pos[2] = 0;
vel[2] = 0;
ned_accel[2] = 0;
}
// Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0)
ned_accel[2] -= GRAVITY;
// Transform the accels back in to body frame
AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0];
accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1];
accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2];
accelsData.temperature = 30;
AccelsSet(&accelsData);
if(baro_offset == 0) {
// Hacky initialization
baro_offset = 50;// * rand_gauss();
} else {
// Very small drift process
baro_offset += rand_gauss() / 100;
}
// Update baro periodically
static uint32_t last_baro_time = 0;
if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) {
BaroAltitudeData baroAltitude;
BaroAltitudeGet(&baroAltitude);
baroAltitude.Altitude = -pos[2] + baro_offset;
BaroAltitudeSet(&baroAltitude);
last_baro_time = PIOS_DELAY_GetRaw();
}
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
static float gps_vel_drift[3] = {0,0,0};
gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0;
gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0;
gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0;
// Update GPS periodically
static uint32_t last_gps_time = 0;
if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) {
// Use double precision here as simulating what GPS produces
double T[3];
T[0] = homeLocation.Altitude+6.378137E6f * DEG2RAD;
T[1] = cosf(homeLocation.Latitude / 10e6 * DEG2RAD)*(homeLocation.Altitude+6.378137E6) * DEG2RAD;
T[2] = -1.0;
static float gps_drift[3] = {0,0,0};
gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0;
gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6);
gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6);
gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]);
gpsPosition.Groundspeed = sqrtf(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2));
gpsPosition.Heading = 180 / M_PI * atan2f(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]);
gpsPosition.Satellites = 7;
gpsPosition.PDOP = 1;
GPSPositionSet(&gpsPosition);
last_gps_time = PIOS_DELAY_GetRaw();
}
// Update GPS Velocity measurements
static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond
if(PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) {
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
gpsVelocity.North = vel[0] + gps_vel_drift[0];
gpsVelocity.East = vel[1] + gps_vel_drift[1];
gpsVelocity.Down = vel[2] + gps_vel_drift[2];
GPSVelocitySet(&gpsVelocity);
last_gps_vel_time = PIOS_DELAY_GetRaw();
}
// Update mag periodically
static uint32_t last_mag_time = 0;
if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) {
MagnetometerData mag;
mag.x = 100+homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2];
mag.y = 100+homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2];
mag.z = 100+homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
magOffsetEstimation(&mag);
MagnetometerSet(&mag);
last_mag_time = PIOS_DELAY_GetRaw();
}
AttitudeSimulatedData attitudeSimulated;
AttitudeSimulatedGet(&attitudeSimulated);
attitudeSimulated.q1 = q[0];
attitudeSimulated.q2 = q[1];
attitudeSimulated.q3 = q[2];
attitudeSimulated.q4 = q[3];
Quaternion2RPY(q,&attitudeSimulated.Roll);
attitudeSimulated.Position[0] = pos[0];
attitudeSimulated.Position[1] = pos[1];
attitudeSimulated.Position[2] = pos[2];
attitudeSimulated.Velocity[0] = vel[0];
attitudeSimulated.Velocity[1] = vel[1];
attitudeSimulated.Velocity[2] = vel[2];
AttitudeSimulatedSet(&attitudeSimulated);
}
static void simulateModelQuadcopter()
{
static double pos[3] = {0,0,0};
static double vel[3] = {0,0,0};
static double ned_accel[3] = {0,0,0};
static float q[4] = {1,0,0,0};
static float rpy[3] = {0,0,0}; // Low pass filtered actuator
static float baro_offset = 0.0f;
static float temperature = 20;
float Rbe[3][3];
const float ACTUATOR_ALPHA = 0.8;
const float MAX_THRUST = GRAVITY * 2;
const float K_FRICTION = 1;
const float GPS_PERIOD = 0.1;
const float MAG_PERIOD = 1.0 / 75.0;
const float BARO_PERIOD = 1.0 / 20.0;
static uint32_t last_time;
float dT = (PIOS_DELAY_DiffuS(last_time) / 1e6);
if(dT < 1e-3)
dT = 2e-3;
last_time = PIOS_DELAY_GetRaw();
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
ActuatorDesiredData actuatorDesired;
ActuatorDesiredGet(&actuatorDesired);
float thrust = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) ? actuatorDesired.Throttle * MAX_THRUST : 0;
if (thrust < 0)
thrust = 0;
if (thrust != thrust)
thrust = 0;
// float control_scaling = thrust * thrustToDegs;
// // In rad/s
// rpy[0] = control_scaling * actuatorDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
// rpy[1] = control_scaling * actuatorDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
// rpy[2] = control_scaling * actuatorDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
//
// GyrosData gyrosData; // Skip get as we set all the fields
// gyrosData.x = rpy[0] * 180 / M_PI + rand_gauss();
// gyrosData.y = rpy[1] * 180 / M_PI + rand_gauss();
// gyrosData.z = rpy[2] * 180 / M_PI + rand_gauss();
RateDesiredData rateDesired;
RateDesiredGet(&rateDesired);
rpy[0] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Roll * (1 - ACTUATOR_ALPHA) + rpy[0] * ACTUATOR_ALPHA;
rpy[1] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Pitch * (1 - ACTUATOR_ALPHA) + rpy[1] * ACTUATOR_ALPHA;
rpy[2] = (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) * rateDesired.Yaw * (1 - ACTUATOR_ALPHA) + rpy[2] * ACTUATOR_ALPHA;
temperature = 20;
GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = rpy[0] + rand_gauss() + (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11; // - powf(temperature - 20,3) * 0.05;;
gyrosData.y = rpy[1] + rand_gauss() + (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11;;
gyrosData.z = rpy[2] + rand_gauss() + (temperature - 20) * 1 + powf(temperature - 20,2) * 0.11;;
gyrosData.temperature = temperature;
GyrosSet(&gyrosData);
// Predict the attitude forward in time
float qdot[4];
qdot[0] = (-q[1] * rpy[0] - q[2] * rpy[1] - q[3] * rpy[2]) * dT * DEG2RAD / 2;
qdot[1] = (q[0] * rpy[0] - q[3] * rpy[1] + q[2] * rpy[2]) * dT * DEG2RAD / 2;
qdot[2] = (q[3] * rpy[0] + q[0] * rpy[1] - q[1] * rpy[2]) * dT * DEG2RAD / 2;
qdot[3] = (-q[2] * rpy[0] + q[1] * rpy[1] + q[0] * rpy[2]) * dT * DEG2RAD / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
if(overideAttitude){
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
attitudeActual.q1 = q[0];
attitudeActual.q2 = q[1];
attitudeActual.q3 = q[2];
attitudeActual.q4 = q[3];
AttitudeActualSet(&attitudeActual);
}
static float wind[3] = {0,0,0};
wind[0] = wind[0] * 0.95 + rand_gauss() / 10.0;
wind[1] = wind[1] * 0.95 + rand_gauss() / 10.0;
wind[2] = wind[2] * 0.95 + rand_gauss() / 10.0;
Quaternion2R(q,Rbe);
// Make thrust negative as down is positive
ned_accel[0] = -thrust * Rbe[2][0];
ned_accel[1] = -thrust * Rbe[2][1];
// Gravity causes acceleration of 9.81 in the down direction
ned_accel[2] = -thrust * Rbe[2][2] + GRAVITY;
// Apply acceleration based on velocity
ned_accel[0] -= K_FRICTION * (vel[0] - wind[0]);
ned_accel[1] -= K_FRICTION * (vel[1] - wind[1]);
ned_accel[2] -= K_FRICTION * (vel[2] - wind[2]);
// Predict the velocity forward in time
vel[0] = vel[0] + ned_accel[0] * dT;
vel[1] = vel[1] + ned_accel[1] * dT;
vel[2] = vel[2] + ned_accel[2] * dT;
// Predict the position forward in time
pos[0] = pos[0] + vel[0] * dT;
pos[1] = pos[1] + vel[1] * dT;
pos[2] = pos[2] + vel[2] * dT;
// Simulate hitting ground
if(pos[2] > 0) {
pos[2] = 0;
vel[2] = 0;
ned_accel[2] = 0;
}
// Sensor feels gravity (when not acceleration in ned frame e.g. ned_accel[2] = 0)
ned_accel[2] -= 9.81;
// Transform the accels back in to body frame
AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = ned_accel[0] * Rbe[0][0] + ned_accel[1] * Rbe[0][1] + ned_accel[2] * Rbe[0][2] + accel_bias[0];
accelsData.y = ned_accel[0] * Rbe[1][0] + ned_accel[1] * Rbe[1][1] + ned_accel[2] * Rbe[1][2] + accel_bias[1];
accelsData.z = ned_accel[0] * Rbe[2][0] + ned_accel[1] * Rbe[2][1] + ned_accel[2] * Rbe[2][2] + accel_bias[2];
accelsData.temperature = 30;
AccelsSet(&accelsData);
if(baro_offset == 0) {
// Hacky initialization
baro_offset = 50;// * rand_gauss();
} else {
// Very small drift process
baro_offset += rand_gauss() / 100;
}
// Update baro periodically
static uint32_t last_baro_time = 0;
if(PIOS_DELAY_DiffuS(last_baro_time) / 1.0e6 > BARO_PERIOD) {
BaroAltitudeData baroAltitude;
BaroAltitudeGet(&baroAltitude);
baroAltitude.Altitude = -pos[2] + baro_offset;
BaroAltitudeSet(&baroAltitude);
last_baro_time = PIOS_DELAY_GetRaw();
}
HomeLocationData homeLocation;
HomeLocationGet(&homeLocation);
static float gps_vel_drift[3] = {0,0,0};
gps_vel_drift[0] = gps_vel_drift[0] * 0.65 + rand_gauss() / 5.0;
gps_vel_drift[1] = gps_vel_drift[1] * 0.65 + rand_gauss() / 5.0;
gps_vel_drift[2] = gps_vel_drift[2] * 0.65 + rand_gauss() / 5.0;
// Update GPS periodically
static uint32_t last_gps_time = 0;
if(PIOS_DELAY_DiffuS(last_gps_time) / 1.0e6 > GPS_PERIOD) {
// Use double precision here as simulating what GPS produces
double T[3];
T[0] = homeLocation.Altitude+6.378137E6f * DEG2RAD;
T[1] = cosf(homeLocation.Latitude / 10e6 * DEG2RAD)*(homeLocation.Altitude+6.378137E6) * DEG2RAD;
T[2] = -1.0;
static float gps_drift[3] = {0,0,0};
gps_drift[0] = gps_drift[0] * 0.95 + rand_gauss() / 10.0;
gps_drift[1] = gps_drift[1] * 0.95 + rand_gauss() / 10.0;
gps_drift[2] = gps_drift[2] * 0.95 + rand_gauss() / 10.0;
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
gpsPosition.Latitude = homeLocation.Latitude + ((pos[0] + gps_drift[0]) / T[0] * 10.0e6);
gpsPosition.Longitude = homeLocation.Longitude + ((pos[1] + gps_drift[1])/ T[1] * 10.0e6);
gpsPosition.Altitude = homeLocation.Altitude + ((pos[2] + gps_drift[2]) / T[2]);
gpsPosition.Groundspeed = sqrtf(pow(vel[0] + gps_vel_drift[0],2) + pow(vel[1] + gps_vel_drift[1],2));
gpsPosition.Heading = 180 / M_PI * atan2f(vel[1] + gps_vel_drift[1],vel[0] + gps_vel_drift[0]);
gpsPosition.Satellites = 7;
gpsPosition.PDOP = 1;
gpsPosition.Status = GPSPOSITION_STATUS_FIX3D;
GPSPositionSet(&gpsPosition);
last_gps_time = PIOS_DELAY_GetRaw();
}
// Update GPS Velocity measurements
static uint32_t last_gps_vel_time = 1000; // Delay by a millisecond
if(PIOS_DELAY_DiffuS(last_gps_vel_time) / 1.0e6 > GPS_PERIOD) {
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
gpsVelocity.North = vel[0] + gps_vel_drift[0];
gpsVelocity.East = vel[1] + gps_vel_drift[1];
gpsVelocity.Down = vel[2] + gps_vel_drift[2];
GPSVelocitySet(&gpsVelocity);
last_gps_vel_time = PIOS_DELAY_GetRaw();
}
// Update mag periodically
static uint32_t last_mag_time = 0;
if(PIOS_DELAY_DiffuS(last_mag_time) / 1.0e6 > MAG_PERIOD) {
MagnetometerData mag;
mag.x = homeLocation.Be[0] * Rbe[0][0] + homeLocation.Be[1] * Rbe[0][1] + homeLocation.Be[2] * Rbe[0][2];
mag.y = homeLocation.Be[0] * Rbe[1][0] + homeLocation.Be[1] * Rbe[1][1] + homeLocation.Be[2] * Rbe[1][2];
mag.z = homeLocation.Be[0] * Rbe[2][0] + homeLocation.Be[1] * Rbe[2][1] + homeLocation.Be[2] * Rbe[2][2];
// Run the offset compensation algorithm from the firmware
magOffsetEstimation(&mag);
MagnetometerSet(&mag);
last_mag_time = PIOS_DELAY_GetRaw();
}
AttitudeSimulatedData attitudeSimulated;
AttitudeSimulatedGet(&attitudeSimulated);
attitudeSimulated.q1 = q[0];
attitudeSimulated.q2 = q[1];
attitudeSimulated.q3 = q[2];
attitudeSimulated.q4 = q[3];
Quaternion2RPY(q,&attitudeSimulated.Roll);
attitudeSimulated.Position[0] = pos[0];
attitudeSimulated.Position[1] = pos[1];
attitudeSimulated.Position[2] = pos[2];
attitudeSimulated.Velocity[0] = vel[0];
attitudeSimulated.Velocity[1] = vel[1];
attitudeSimulated.Velocity[2] = vel[2];
AttitudeSimulatedSet(&attitudeSimulated);
}
