BOOL PIDCompute(PIDControl* const pid)
{
FLOAT error, dInput;
if(pid->mode == MANUAL)
{
return false;
}
// The classic PID error term
//error = (pid->setpoint) - (pid->input);
error = pid->calcError(pid->setpoint, pid->input);
// Compute the integral term separately ahead of time
pid->iTerm += (pid->alteredKi) * error;
// Constrain the integrator to make sure it does not exceed output bounds
pid->iTerm = CONSTRAIN( (pid->iTerm), (pid->outMin), (pid->outMax) );
// Take the "derivative on measurement" instead of "derivative on error"
dInput = (pid->input) - (pid->lastInput);
// Run all the terms together to get the overall output
pid->output = pid->alteredKf * pid->setpoint + (pid->alteredKp) * error + (pid->iTerm) - (pid->alteredKd) * dInput;
// Bound the output
pid->output = CONSTRAIN( (pid->output), (pid->outMin), (pid->outMax) );
// Make the current input the former input
pid->lastInput = pid->input;
return true;
}
inline void constrain()
{
r = CONSTRAIN(r, 0.0f, 1.0f);
g = CONSTRAIN(g, 0.0f, 1.0f);
b = CONSTRAIN(b, 0.0f, 1.0f);
a = CONSTRAIN(a, 0.0f, 1.0f);
}
// Values: 0.0f .. 1.0f
explicit Color(const float red, const float green, const float blue, const float alpha = 1.0f)
{
r = CONSTRAIN(red, 0.0f, 1.0f);
g = CONSTRAIN(green, 0.0f, 1.0f);
b = CONSTRAIN(blue, 0.0f, 1.0f);
a = CONSTRAIN(alpha, 0.0f, 1.0f);
}
static const Color& constrain(Color& color)
{
color.r = CONSTRAIN(color.r, 0.0f, 1.0f);
color.g = CONSTRAIN(color.g, 0.0f, 1.0f);
color.b = CONSTRAIN(color.b, 0.0f, 1.0f);
color.a = CONSTRAIN(color.a, 0.0f, 1.0f);
return color;
}
Coordinate AltitudeControl(CoordinateCtrl * coordinateCtrl)
{
/*************Altitude**********************************************************************************************************/
AltitudeError = coordinateCtrl->expectCoord.altitude - coordinateCtrl->realCoord.altitude;
AltitudeErrorIntegral += AltitudeError;
CONSTRAIN(AltitudeErrorIntegral, 10);
coordinateCtrl->coordinateOut.altitude = coordinateCtrl->altitudePID.kp * AltitudeError + coordinateCtrl->altitudePID.ki * AltitudeErrorIntegral + coordinateCtrl->altitudePID.kd * (AltitudeError - AltitudeLastError);
AltitudeLastError = AltitudeError;
CONSTRAIN(coordinateCtrl->coordinateOut.altitude, 0.2);
return coordinateCtrl->coordinateOut;
}
void PIDy_Update(float dt)
{
_ERRy=ERRy;
ERRy=CONSTRAIN(rx_value[1]-pitch,MAX_TARGET_ANGLE);
ERRyI=CONSTRAIN(ERRy*dt+ERRyI,I_MAX);
P_Temp=PID_Value[9]*ERRy;
I_Temp=PID_Value[10]*ERRyI;
D_Temp=CONSTRAIN(-PID_Value[11]*gyro_y_rate,PG_MAX);
PIDy= (int)CONSTRAIN((P_Temp+I_Temp+D_Temp)/1.414,PID_OUT_MAX);
}
void PIDx_Update(float dt)
{
_ERRx=ERRx;
ERRx=CONSTRAIN(rx_value[0]-roll,MAX_TARGET_ANGLE);
ERRxI=CONSTRAIN(ERRx*dt+ERRxI,I_MAX);
P_Temp=PID_Value[3]*ERRx;
I_Temp=PID_Value[4]*ERRxI;
D_Temp=CONSTRAIN(-PID_Value[5]*gyro_x_rate,PG_MAX);
PIDx=(int)CONSTRAIN((P_Temp+I_Temp+D_Temp)/1.414,PID_OUT_MAX);
}
// input: rcData , raw data from remote control source
// output: RC_DATA, desired thro, pitch, roll, yaw
void RCDataProcess(void)
{
CONSTRAIN(rcData[THROTTLE],1000,2000);
CONSTRAIN(rcData[YAW],1000,2000);
CONSTRAIN(rcData[PITCH],1000,2000);
CONSTRAIN(rcData[ROLL],1000,2000);
// CUT_DB(rcData[YAW],1500,APP_YAW_DB);
// CUT_DB(rcData[PITCH],1500,APP_PR_DB);
// CUT_DB(rcData[ROLL],1500,APP_PR_DB);
RC_DATA.THROTTLE=rcData[THROTTLE]-1000;
// RC_DATA.YAW=(rcData[YAW]-1500)/500.0 *Angle_Max;
// RC_DATA.PITCH=(rcData[PITCH]-1500)/500.0 *Angle_Max; // + Pitch_error_init;
// RC_DATA.ROOL=(rcData[ROLL]-1500)/500.0 *Angle_Max; // + Rool_error_init;
RC_DATA.YAW= YAW_RATE_MAX * dbScaleLinear((rcData[YAW] - 1500),500,APP_YAW_DB);
RC_DATA.PITCH= Angle_Max * dbScaleLinear((rcData[PITCH] - 1500),500,APP_PR_DB);
RC_DATA.ROOL= Angle_Max * dbScaleLinear((rcData[ROLL] - 1500),500,APP_PR_DB);
switch(armState)
{
case REQ_ARM:
if(IMUCheck() && !Battery.alarm)
{
armState=ARMED;
FLY_ENABLE=0xA5;
}
else
{
FLY_ENABLE=0;
armState=DISARMED;
}
break;
case REQ_DISARM:
FLY_ENABLE=0;
altCtrlMode=MANUAL; //上锁后加的处理
zIntReset=1; //
thrustZSp=0;
thrustZInt=HOVER_THRU;
offLandFlag=0;
armState=DISARMED;
break;
default:
break;
}
}
void PIDyp_Update(float dt)
{
float tempy = -0.04f;
_ERRyp=ERRyp;
ERRyp=CONSTRAIN(rx_value[4]-Y, MAX_DELTA_Y);
ERRypI=CONSTRAIN(ERRyp*dt+ERRypI, I_P_MAX);
P_Temp=PID_Value[6]*ERRyp;
I_Temp=PID_Value[7]*ERRypI;
D_Temp=CONSTRAIN(-PID_Value[8]*rdy,PG_MAX);
PIDyp=(int)CONSTRAIN((P_Temp+I_Temp+D_Temp),PID_Y_MAX);
rx_value[1]=tempy*PIDyp;
}
long update_pid_controller(pid_controller_t* controller, pid_state_t* state, long setpoint, long measured_value)
{
long time = nPgmTime;
long elapsed_msec = time - state->prev_time;
// If no measurable time has passed, we can't divide by zero, so return the last output value and don't update anything
if (elapsed_msec <= 0)
{
//writeDebugStreamLine("elapsed_msec == 0!");
return state->prev_output;
}
long error = setpoint - measured_value;
if (abs(error) > controller->CLOSE_ENOUGH && state->integral < controller->MAX_INTEGRAL)
state->integral += error * elapsed_msec;
float derivative = (float)(error - state->prev_error) / elapsed_msec;
//writeDebugStreamLine("measured_value: %d integral: %.2f derivative: %.2f", measured_value, state->integral, derivative);
float output =
controller->KP * error
+ controller->KI * state->integral
+ controller->KD * derivative;
//writeDebugStreamLine("output before correction: %.0f", output);
output = CONSTRAIN(output,
state->prev_output - controller->MAX_CHANGE,
state->prev_output + controller->MAX_CHANGE);
//writeDebugStreamLine("output after correction 1: %.0f", output);
if (output < -1)
output = CONSTRAIN(output, controller->MIN_OUTPUT, -controller->EFFECTIVE_MIN);
else if (output > 1)
output = CONSTRAIN(output, controller->EFFECTIVE_MIN, controller->MAX_OUTPUT);
else
output = 0;
//writeDebugStreamLine("output after correction: %d", (long)output);
state->prev_error = error;
state->prev_output = output;
state->prev_derivative = derivative;
state->prev_time = time;
return output;
}
static int
ambitv_lpd8806_map_output_to_point(
struct ambitv_sink_component* component,
int output,
int width,
int height,
int* x,
int* y)
{
int ret = -1, *outp = NULL, str_idx = 0, led_idx = 0;
struct ambitv_lpd8806_priv* lpd8806 =
(struct ambitv_lpd8806_priv*)component->priv;
outp = ambitv_lpd8806_ptr_for_output(lpd8806, output, &str_idx, &led_idx);
if (NULL != outp) {
ret = 0;
float llen = lpd8806->led_len[str_idx] - 1;
float dim = (str_idx < 2) ? width : height;
float inset = lpd8806->led_inset[str_idx] * dim;
dim -= 2*inset;
switch (str_idx) {
case 0: // top
*x = (int)CONSTRAIN(inset + (dim / llen) * led_idx, 0, width);
*y = 0;
break;
case 1: // bottom
*x = (int)CONSTRAIN(inset + (dim / llen) * led_idx, 0, width);
*y = height;
break;
case 2: // left
*x = 0;
*y = (int)CONSTRAIN(inset + (dim / llen) * led_idx, 0, height);
break;
case 3: // right
*x = width;
*y = (int)CONSTRAIN(inset + (dim / llen) * led_idx, 0, height);
break;
default:
ret = -1;
break;
}
} else {
*x = *y = -1;
}
return ret;
}
void PIDxp_Update(float dt)
{
float tempx = -0.04f;
_ERRxp=ERRxp;
ERRxp=CONSTRAIN(rx_value[3]-X, MAX_DELTA_X);
ERRxpI=CONSTRAIN(ERRxp*dt+ERRxpI, I_P_MAX);
P_Temp=PID_Value[0]*ERRxp;
I_Temp=PID_Value[1]*ERRxpI;
D_Temp=CONSTRAIN(-PID_Value[3]*rdx,PG_MAX);
PIDxp=(int)CONSTRAIN((P_Temp+I_Temp+D_Temp),PID_X_MAX);
rx_value[0]=tempx*PIDxp;
}
int zrcar_wheel_r_set(int speed)
{
int fd = open(zrcar_dev.wheel_dev,O_RDWR);
TEST(fd > 0,"wheel device open failed!\n");
CONSTRAIN(speed,-45,45);
ioctl(fd,WHEEL_R_SET,speed);
close(fd);
return 0;
}
void PIDz_Update(float dt)
{
if(rx_value[2]==0){
//auto hold mode, increas PIDz_Out 0.5 by one step to prevent ocilation
PIDz_Out+=0.8;
if(PIDz_Out>PID_Z_MAX)PIDz_Out=PID_Z_MAX;
}else{
// control z axis, just need small output
PIDz_Out=PID_Z_MIN;
}
_ERRz=ERRz;
ERRz=CONSTRAIN(rx_value[2]-gyro_z_rate,MAX_GYRO_ERROR);
ERRzD=(ERRz-_ERRz)/dt;
P_Temp=CONSTRAIN(PID_Value[15]*ERRz,PG_MAX);
D_Temp=PID_Value[17]*ERRzD;
PIDz= (int) CONSTRAIN(P_Temp+D_Temp,PIDz_Out);
}
void PIDOutputLimitsSet(PIDControl* const pid, FLOAT min, FLOAT max)
{
// Check if the parameters are valid
if(min >= max)
{
return;
}
// Save the parameters
pid->outMin = min;
pid->outMax = max;
// If in automatic, apply the new constraints
if(pid->mode == AUTOMATIC)
{
pid->output = CONSTRAIN(pid->output, min, max);
pid->iTerm = CONSTRAIN(pid->iTerm, min, max);
}
}
void PIDzp_Update(float dt)
{
float tempz = 0.003f;
_ERRzp=ERRzp;
ERRzp=CONSTRAIN(rx_value[5]-height, MAX_DELTA_Z);
ERRzpI=CONSTRAIN(ERRzp*dt+ERRzpI, I_H_MAX);
RateZ=CONSTRAIN((ERRzp-_ERRzp)/dt,MAX_Z_RATE);
P_Temp=PID_Value[12]*ERRzp;
I_Temp=PID_Value[13]*ERRzpI;
D_Temp=CONSTRAIN(PID_Value[14]*RateZ,PG_MAX);
PIDzp=(int)MINMAX((P_Temp+I_Temp+D_Temp)+100,100,PID_H_OUT_MAX);
/*myprintf("ErrZ:%f\tErrZI:%f\tRateZ:%f\t\r\n",ERRzp,ERRzpI,RateZ);*/
/*myprintf("P:%f\tI:%f\tD:%f\t\r\n",P_Temp,I_Temp,D_Temp);*/
/*myprintf("PID:%f\r\n",PIDzp);*/
throttle=tempz*PIDzp;
avg_height = (height + _height) * 0.5f;
_height = height;
}
void Tooltip_Redraw(Tooltip *me)
{
if (!me->bShown || (0 == WSTRLEN(me->pwsz))) {
return;
}
me->rc.dx = IDISPLAY_MeasureText(me->piDisplay,AEE_FONT_NORMAL,me->pwsz)+4;
me->rc.x = me->nX + (((me->nDx-me->rc.dx)*me->nXPercent)/100);
me->rc.x = CONSTRAIN(me->rc.x, me->nX, (me->nX + me->nDx) - me->rc.dx);
IDISPLAY_DrawText(me->piDisplay,AEE_FONT_NORMAL,me->pwsz,-1,
me->rc.x+2,me->rc.y+2,
&me->rc,IDF_RECT_FILL|IDF_RECT_FRAME);
}
void PIDModeSet(PIDControl* const pid, PIDMode mode)
{
// If the mode changed from MANUAL to AUTOMATIC
if(pid->mode != mode && mode == AUTOMATIC)
{
// Initialize a few PID parameters to new values
pid->iTerm = pid->output;
pid->lastInput = pid->input;
// Constrain the integrator to make sure it does not exceed output bounds
pid->iTerm = CONSTRAIN( (pid->iTerm), (pid->outMin), (pid->outMax) );
}
pid->mode = mode;
}
void I2S_RX_CallBack(int16_t *src, int16_t *dst, int16_t sz)
{
float samplespeed;
static u16 cc;
for (u8 x=0;x<5;x++){
float value=(float)adc_buffer[transform[x].whichone]/65536.0f; // 4096.0f; // why 65536.0f as in clouds - align? - try that
if (transform[x].flip) {
value = 1.0f - value;
}
smoothed_adc_value[x] += transform[x].filter_coeff * (value - smoothed_adc_value[x]);
}
_speed=smoothed_adc_value[SPEED_];
CONSTRAIN(_speed,0.0f,1.0f);
_selx=smoothed_adc_value[SELX_];
CONSTRAIN(_selx,0.0f,1.0f);
_sely=smoothed_adc_value[SELY_];
CONSTRAIN(_sely,0.0f,1.0f);
_selz=smoothed_adc_value[SELZ_];
CONSTRAIN(_selz,0.0f,1.0f);
_mode=smoothed_adc_value[MODE_];
CONSTRAIN(_mode,0.0f,1.0f);
_intmode=_mode*transform[MODE_].multiplier; //0=32 we hope!
samplespeed=_speed+0.01f; // TODO test this fully!
_intspeed=_speed*transform[SPEED_].multiplier;
// splitting input
for (u8 x=0;x<sz/2;x++){
sample_buffer[x]=*(src++); // right is input on LACH, LEFT ON EURO!
src++;
}
_intmode=15; // 15-> test_wave // checked=0,1,2,3,4,5,6,7,8 17 is last
void (*generators[])(int16_t* incoming, int16_t* outgoing, float samplespeed, u8 size)={tms5220talkie, fullklatt, sp0256, simpleklatt, sammy, tms5200mame, tubes, channelv, testvoc, digitalker, nvp, nvpSR, foffy, voicformy, lpc_error, test_wave, wormas_wave, test_worm_wave};
generators[_intmode](sample_buffer,mono_buffer,samplespeed,sz/2);
// copy sample buffer into audio_buffer as COMPOST
if (!toggled){
for (u8 x=0;x<sz/2;x++) {
audio_buffer[cc]=mono_buffer[x];
cc++;
if (cc>AUDIO_BUFSZ) cc=0;
}
}
/*
for (x=0;x<sz/2;x++){ // STRIP_OUT - leave for TESTING
readpos=samplepos;
mono_buffer[x]=audio_buffer[readpos];
samplepos+=samplespeed;
if (readpos>=ending) samplepos=0.0f;
}
*/
#ifdef TEST
audio_comb_stereo(sz, dst, left_buffer, mono_buffer);
#else // left is out on our WORM BOARD!
audio_comb_stereo(sz, dst, mono_buffer,left_buffer);
#endif
}
void point_to_box(int *x, int *y, int w, int h, int width, int height)
{
*x = CONSTRAIN(*x-w, 0, width);
*y = CONSTRAIN(*y-h, 0, height);
}
static int ambitv_mood_light_processor_update_sink(struct ambitv_processor_component* processor,
struct ambitv_sink_component* sink)
{
int i, n_out, ret = 0;
struct ambitv_mood_light_processor* mood = (struct ambitv_mood_light_processor*) processor->priv;
if (sink->f_num_outputs && sink->f_map_output_to_point && sink->f_set_output_to_rgb)
{
int x, y, r, g, b;
float f;
n_out = sink->f_num_outputs(sink);
switch(mood->mode)
{
case 0:
{
for (i = 0; i < n_out; i++)
{
ret = sink->f_map_output_to_point(sink, i, 1600, 900, &x, &y);
f = CONSTRAIN((x + y) / 2500.0, 0.0, 1.0);
f = fmod(f + mood->offset, 1.0);
ambitv_hsl_to_rgb(255 * f, 255, 128, &r, &g, &b);
sink->f_set_output_to_rgb(sink, i, r, g, b);
}
}
break;
case 1:
{
for (i = 0; i < n_out; i++)
{
ret = sink->f_map_output_to_point(sink, i, 1600, 900, &x, &y);
if(y < 0.0001)
f = CONSTRAIN(x / 5000.0, 0.0, 1.0);
else if(x > 1599.9999)
f = CONSTRAIN((1600.0 + y) / 5000.0, 0.0, 1.0);
else if(y > 899.9999)
f = CONSTRAIN((4100.0 - x) / 5000.0, 0.0, 1.0);
else
f = CONSTRAIN((5000.0 - y) / 5000.0, 0.0, 1.0);
f = fmod(f + mood->offset, 1.0);
ambitv_hsl_to_rgb(255 * f, 255, 128, &r, &g, &b);
sink->f_set_output_to_rgb(sink, i, r, g, b);
}
}
break;
case 2:
{
ambitv_hsl_to_rgb(255 * mood->offset, 255, 128, &r, &g, &b);
for (i = 0; i < n_out; i++)
{
sink->f_set_output_to_rgb(sink, i, r, g, b);
}
}
break;
}
}
else
ret = -1;
if (sink->f_commit_outputs)
sink->f_commit_outputs(sink);
return ret;
}
static sns_ddf_status_e sns_dd_acc_bma150_cal_do_calibrate
(
sns_ddf_handle_t port_handle,
uint32_t uMaximumTries,
const AccDataType* accReference_ptr,
boolean write_to_eeprom_b,
OffsetDataType* rc_offset_ptr
)
{
OffsetDataType stOriginalOffset;
GainDataType stGain;
OffsetDataType stOffset;
uint32_t uTryCount;
int16_t nsDeltaX;
int16_t nsDeltaY;
int16_t nsDeltaZ;
sns_ddf_status_e stat;
// Get original offset values
stat = sns_dd_acc_bma150_cal_get_offset_gain_axes( port_handle, &stOriginalOffset, &stGain );
if ( SNS_DDF_SUCCESS != stat )
{
goto do_calib_bail;
}
// Start offset values from original values
stOffset.usX = stOriginalOffset.usX;
stOffset.usY = stOriginalOffset.usY;
stOffset.usZ = stOriginalOffset.usZ;
/**
* The following loop does the actual calibration. The device is calibrated
* if the difference between all axis acceleration data and the reference
* data is within tolerance.
* The general outline is:
* 1. Get average data reading. If reading is not stable try again (for a
* maximum of BOSCH_ACCEL_SENSOR_MAX_NUM_OF_AVG_TRIES)
* 2. Check if the device is calibrated, if so end.
* 3. Calculate new offset values, write them and start over.
*
* This is done for a maximum of uMaximumTries times.
*/
for ( uTryCount = 0 ; uTryCount < uMaximumTries ; ++uTryCount )
{
int32_t nGetAvgCounter = 0;
boolean bDataIsStable;
AccDataType accAverage;
// Try to get stable average data reading
do
{
stat = sns_dd_acc_bma150_cal_get_avg_data( port_handle,
&bDataIsStable,
&accAverage,
BOSCH_ACCEL_SENSOR_NUMBER_OF_READS_FOR_AVG );
if ( SNS_DDF_SUCCESS != stat )
{
goto do_calib_bail;
}
++nGetAvgCounter;
} while ( FALSE == bDataIsStable &&
nGetAvgCounter < BOSCH_ACCEL_SENSOR_MAX_NUM_OF_AVG_TRIES );
// Check if we got stable data
if ( FALSE == bDataIsStable )
{
stat = SNS_DDF_EFAIL;
goto do_calib_bail;
}
// Calculate delta between average value and reference
nsDeltaX = accAverage.nsX - accReference_ptr->nsX;
nsDeltaY = accAverage.nsY - accReference_ptr->nsY;
nsDeltaZ = accAverage.nsZ - accReference_ptr->nsZ;
// If offset error is outside tolerance recalculate the offset
if ( ABS( nsDeltaX ) > BOSCH_ACCEL_SENSOR_OFFSET_TOLERANCE_2G ||
ABS( nsDeltaY ) > BOSCH_ACCEL_SENSOR_OFFSET_TOLERANCE_2G ||
ABS( nsDeltaZ ) > BOSCH_ACCEL_SENSOR_OFFSET_TOLERANCE_2G )
{
/**
* Calculate new offsets.
* For 2G range, each LSB in offset changes output value by
* BOSCH_ACCEL_SENSOR_OFFSET_PER_LSB_2G so divide by this value.
* Also make sure values are between 0 and BOSCH_ACCEL_MAX_UINT_10BIT
*/
int32_t nTemp = stOffset.usX -
sns_dd_acc_bma150_cal_divide_and_round( nsDeltaX, BOSCH_ACCEL_SENSOR_OFFSET_PER_LSB_2G );
stOffset.usX = ( uint16_t )CONSTRAIN( nTemp, 0, BOSCH_ACCEL_MAX_UINT_10BIT );
nTemp = stOffset.usY -
sns_dd_acc_bma150_cal_divide_and_round( nsDeltaY, BOSCH_ACCEL_SENSOR_OFFSET_PER_LSB_2G );
stOffset.usY = ( uint16_t )CONSTRAIN( nTemp, 0, BOSCH_ACCEL_MAX_UINT_10BIT );
nTemp = stOffset.usZ -
sns_dd_acc_bma150_cal_divide_and_round( nsDeltaZ, BOSCH_ACCEL_SENSOR_OFFSET_PER_LSB_2G );
stOffset.usZ = ( uint16_t )CONSTRAIN( nTemp, 0, BOSCH_ACCEL_MAX_UINT_10BIT );
// Write new offsets to image
stat = sns_dd_acc_bma150_cal_set_offset_gain_axes( port_handle, &stOffset, &stGain );
if ( SNS_DDF_SUCCESS != stat )
{
goto do_calib_bail;
}
// Wait until filter chain is filled by values with new offset settings
sns_ddf_delay( BOSCH_ACCEL_SENSOR_FILTER_CHAIN_FILL_DELAY_USEC );
}
else
{
// Device is calibrated (values are currently only in image).
// raise sampled sensor data
rc_offset_ptr->usX = stOffset.usX;
rc_offset_ptr->usY = stOffset.usY;
rc_offset_ptr->usZ = stOffset.usZ;
// If we don't want to actually write to the EEPROM just return.
if ( FALSE == write_to_eeprom_b )
{
stat = SNS_DDF_SUCCESS;
goto do_calib_bail;
}
// Write new X offset value to EEPROM (only if it changed).
if ( stOffset.usX != stOriginalOffset.usX )
{
stat = sns_dd_acc_bma150_cal_set_offset_gain( port_handle,
BOSCH_ACCEL_AXIS_X_EEPROM,
stOffset.usX,
stGain.ucX );
if (SNS_DDF_SUCCESS != stat)
{
goto do_calib_bail;
}
}
// Write new Y offset value to EEPROM (only if it changed).
if ( stOffset.usY != stOriginalOffset.usY )
{
stat = sns_dd_acc_bma150_cal_set_offset_gain( port_handle,
BOSCH_ACCEL_AXIS_Y_EEPROM,
stOffset.usY,
stGain.ucY );
if (SNS_DDF_SUCCESS != stat)
{
goto do_calib_bail;
}
}
// Write new Z offset value to EEPROM (only if it changed).
if ( stOffset.usZ != stOriginalOffset.usZ )
{
stat = sns_dd_acc_bma150_cal_set_offset_gain( port_handle,
BOSCH_ACCEL_AXIS_Z_EEPROM,
stOffset.usZ,
stGain.ucZ );
if (SNS_DDF_SUCCESS != stat)
{
goto do_calib_bail;
}
}
stat = SNS_DDF_SUCCESS; // Device is now calibrated
goto do_calib_bail;
}
} // !for
do_calib_bail:
return stat;
}
/**
* Entry point called from boot.S for bootstrap processor.
*/
void arch_init(uint32_t board_id,
struct atag *atag_base,
lvaddr_t elf_file,
lvaddr_t alloc_top)
{
//
// Assumptions:
//
// - MMU and caches are enabled. No lockdowns in caches or TLB.
// - Kernel has own section starting at KERNEL_OFFSET.
// - Kernel section includes the highmem relocated exception vector table.
//
struct atag * ae = NULL;
exceptions_init();
ae = atag_find(atag_base, ATAG_MEM);
paging_map_memory(0, ae->u.mem.start, ae->u.mem.bytes);
ae = atag_find(atag_base, ATAG_CMDLINE);
if (ae != NULL)
{
parse_commandline(ae->u.cmdline.cmdline, cmdargs);
tick_hz = CONSTRAIN(tick_hz, 10, 1000);
}
if (board_id == hal_get_board_id())
{
errval_t errval;
serial_console_init(true);
// do not remove/change this printf: needed by regression harness
printf("Barrelfish CPU driver starting on ARMv5 Board id 0x%08"PRIx32"\n",
board_id);
printf("The address of paging_map_kernel_section is %p\n",
paging_map_kernel_section);
errval = serial_debug_init();
if (err_is_fail(errval))
{
printf("Failed to initialize debug port: %d", serial_debug_port);
}
debug(SUBSYS_STARTUP, "alloc_top %08"PRIxLVADDR" %08"PRIxLVADDR"\n",
alloc_top, alloc_top - KERNEL_OFFSET);
debug(SUBSYS_STARTUP, "elf_file %08"PRIxLVADDR"\n", elf_file);
my_core_id = hal_get_cpu_id();
extern struct kcb bspkcb;
memset(&bspkcb, 0, sizeof(bspkcb));
kcb_current = &bspkcb;
pic_init();
pit_init(tick_hz);
tsc_init();
ae = atag_find(atag_base, ATAG_MEM);
// Add unused physical memory to memory map
phys_mmap_t phys_mmap;
// Kernel effectively consumes [0...alloc_top]
// Add region above alloc_top with care to skip exception vector
// page.
if (alloc_top < ETABLE_ADDR) {
phys_mmap_add(&phys_mmap,
alloc_top - KERNEL_OFFSET,
ETABLE_ADDR - KERNEL_OFFSET);
}
phys_mmap_add(&phys_mmap,
ETABLE_ADDR - KERNEL_OFFSET + BASE_PAGE_SIZE,
ae->u.mem.start + ae->u.mem.bytes);
ae = atag_find(atag_base, ATAG_VIDEOLFB);
if (NULL != ae)
{
// Remove frame buffer (if present).
phys_mmap_remove(&phys_mmap,
ae->u.videolfb.lfb_base,
ae->u.videolfb.lfb_base + ae->u.videolfb.lfb_size);
assert(!"Not supported");
}
ae = atag_find(atag_base, ATAG_INITRD2);
if (NULL != ae)
{
phys_mmap_remove(&phys_mmap,
ae->u.initrd2.start,
ae->u.initrd2.start + ae->u.initrd2.bytes);
arm_kernel_startup(&phys_mmap,
ae->u.initrd2.start,
ae->u.initrd2.bytes);
}
else {
panic("initrd not found\n");
}
}
else {
panic("Mis-matched board id: [current %"PRIu32", kernel %"PRIu32"]",
board_id, hal_get_board_id());
}
}
Attitude AttitudeControl(AttitudeCtrl * attitudeCtrl)
{
if (attitudeCtrl->realAgl.err_flag == 0)
{
/***********外环控制*********************************************************************************************************/
//采用位置型PID，积分采用梯形积分, 计算外环误差（PD）
//ShellPitch
shellPitchError = attitudeCtrl->expectAgl.pitch - attitudeCtrl->realAgl.pitch;
shellPitchErrorIntegral += shellPitchError;
attitudeCtrl->attitudeOut.pitch = attitudeCtrl->shellPitchPID.kp * shellPitchError + attitudeCtrl->shellPitchPID.ki * shellPitchErrorIntegral + attitudeCtrl->shellPitchPID.kd * (shellPitchError - shellPitchLastError);
shellPitchLastError = shellPitchError;
CONSTRAIN(shellPitchErrorIntegral, INTEGRALMAX);
if (RecvCom[0] < THRMIN)
{
shellPitchErrorIntegral = 0.0;
}
//ShellRoll
shellRollError = attitudeCtrl->expectAgl.roll - attitudeCtrl->realAgl.roll;
shellRollErrorIntegral += shellRollError;
attitudeCtrl->attitudeOut.roll = attitudeCtrl->shellRollPID.kp * shellRollError + attitudeCtrl->shellRollPID.ki * shellRollErrorIntegral + attitudeCtrl->shellRollPID.kd * (shellRollError - shellRollLastError);
shellRollLastError = shellRollError;
CONSTRAIN(shellRollErrorIntegral, INTEGRALMAX);
if (RecvCom[0] < THRMIN)
{
shellRollErrorIntegral = 0.0;
}
//ShellYaw
shellYawError = attitudeCtrl->expectAgl.yaw - attitudeCtrl->realAgl.yaw;
shellYawErrorIntegral += shellYawError;
attitudeCtrl->attitudeOut.yaw = attitudeCtrl->shellYawPID.kp * shellYawError + attitudeCtrl->shellYawPID.ki * shellYawErrorIntegral + attitudeCtrl->shellYawPID.kd * (shellYawError - shellYawLastError);
shellYawLastError = shellYawError;
CONSTRAIN(shellYawErrorIntegral, INTEGRALMAX);
if (RecvCom[0] < THRMIN)
{
shellYawErrorIntegral = 0.0;
}
/*************内环控制**********************************************************************************************************/
//CorePitch
corePitchError = attitudeCtrl->attitudeOut.pitch - (attitudeCtrl->realAgl.pitch - lastRealAgl.pitch);
lastRealAgl.pitch = attitudeCtrl->realAgl.pitch;
corePitchErrorIntegral += corePitchError;
CONSTRAIN(corePitchErrorIntegral, INTEGRALMAX);
/*************低油门积分清零**********************************************************************************************************/
if (RecvCom[0] < THRMIN)
{
corePitchErrorIntegral = 0.0;
}
attitudeCtrl->attitudeOut.pitch = attitudeCtrl->corePitchPID.kp * corePitchError + attitudeCtrl->corePitchPID.ki * corePitchErrorIntegral + attitudeCtrl->corePitchPID.kd * (corePitchError - corePitchLastError);
corePitchLastError = corePitchError;
if (attitudeCtrl->attitudeOut.pitch > 0.25)
{
attitudeCtrl->attitudeOut.pitch = 0.25;
}
//CoreRoll
coreRollError = attitudeCtrl->attitudeOut.roll - (attitudeCtrl->realAgl.roll - lastRealAgl.roll);
lastRealAgl.roll = attitudeCtrl->realAgl.roll;
coreRollErrorIntegral += coreRollError;
CONSTRAIN(coreRollErrorIntegral, INTEGRALMAX);
/*************低油门积分清零**********************************************************************************************************/
if (RecvCom[0] < THRMIN)
{
coreRollErrorIntegral = 0.0;
}
attitudeCtrl->attitudeOut.roll = attitudeCtrl->coreRollPID.kp * coreRollError + attitudeCtrl->coreRollPID.ki * coreRollErrorIntegral + attitudeCtrl->coreRollPID.kd * (coreRollError - coreRollLastError);
coreRollLastError = coreRollError;
if (attitudeCtrl->attitudeOut.roll > 0.25)
{
attitudeCtrl->attitudeOut.roll = 0.25;
}
// //CoreYaw
// coreYawError = attitudeCtrl->attitudeOut.yaw - (attitudeCtrl->realAgl.yaw - lastRealAgl.yaw);
// lastRealAgl.yaw = attitudeCtrl->realAgl.yaw;
// coreYawErrorIntegral += coreYawError;
// /*************积分限幅环节**********************************************************************************************************/
// if (coreYawErrorIntegral > INTEGRALMAX)
// {
// coreYawErrorIntegral = INTEGRALMAX;
// }
// if (coreYawErrorIntegral < -INTEGRALMAX)
// {
// coreYawErrorIntegral = -INTEGRALMAX;
// }
// /*************低油门积分清零**********************************************************************************************************/
// if (RecvCom[0] < THRMIN)
// {
// coreYawErrorIntegral = 0.0;
// }
// attitudeCtrl->attitudeOut.yaw = attitudeCtrl->corePid.yawKp * coreYawError + attitudeCtrl->corePid.yawKi * coreYawErrorIntegral + attitudeCtrl->corePid.yawKd * (coreYawError - coreYawLastError);
// coreYawLastError = coreYawError;
if (attitudeCtrl->attitudeOut.yaw > 0.25)
{
attitudeCtrl->attitudeOut.yaw = 0.25;
}
lastAttitudeOut = attitudeCtrl->attitudeOut;
return attitudeCtrl->attitudeOut;
}
else
{
return lastAttitudeOut;
}
}
