float adc_read(int channel)
{
switch(channel)
{
case 0:
return adcarray[0] ;
//break;
case 1:
return mapf( (float) adcarray[1] , 2727.0 , 3050.0 , 3.77 , 4.22);
//break;
case 2:
return adcarray[2];
//break;
case 3:
return adcarray[3];
//break;
default:
return 0;
//break;
}
}
static int snmp_map_ifs(struct ifstat_driver *driver,
int (*mapf)(struct ifstat_driver *driver, int nifaces, void *pdata),
void *pdata) {
struct snmp_driver_data *data = driver->data;
struct ifsnmp *ifsnmp = data->ifsnmp;
int ifaces, nifaces, index, i;
if ((ifaces = snmp_get_ifcount(data->session)) <= 0) {
ifstat_error("snmp: no interfaces returned.");
return 0;
}
for(index = 0; index <= (ifaces /data->num_ifsreqs); index++) {
nifaces = ifaces - index * data->num_ifsreqs;
if (nifaces > data->num_ifsreqs)
nifaces = data->num_ifsreqs;
for (i = 0; i < nifaces; i++)
ifsnmp[i].index = index * data->num_ifsreqs + i + 1;
if(!mapf(driver, nifaces, pdata))
return 0;
}
return 1;
}
static int kvm_map_ifs(struct ifstat_driver *driver,
int (*mapf)(char *name, struct ifnet *ifnet, void *data),
void *mdata) {
struct kvm_driver_data *data = driver->data;
unsigned long ifaddr;
struct ifnet ifnet;
#ifndef HAVE_IFNET_IF_XNAME
char ifname[IFNAMSIZ + 1];
#endif
char interface[IFNAMSIZ + 10];
for (ifaddr = data->ifnetaddr; ifaddr != 0;
ifaddr = (unsigned long) IFNET_NEXT(ifnet)) {
if (kvm_read(data->kvmfd, ifaddr, &ifnet, sizeof(ifnet)) < 0) {
ifstat_error("kvm_read: %s", data->errbuf);
return 0;
}
#ifdef HAVE_IFNET_IF_XNAME
memcpy(interface, ifnet.if_xname, sizeof(interface));
interface[sizeof(interface) - 1] = '\0';
#else
if (kvm_read(data->kvmfd, (unsigned long) ifnet.if_name, &ifname, sizeof(ifname)) < 0) {
ifstat_error("kvm_read: %s", data->errbuf);
return 0;
}
ifname[sizeof(ifname) - 1] = '\0';
sprintf(interface, "%s%d", ifname, ifnet.if_unit);
#endif
if (!mapf(interface, &ifnet, mdata))
return 0;
}
return 1;
}
void RefreshColorArray()
{
for (int i = 0; i < SIZE; i++)
{
gray[i] = mapf(i, 0, SIZE, 0, 1);
alpha[i] = 1;
}
}
static int dlpi_map_ifs(struct dlpi_driver_data *dlpi,
int (*mapf)(mib_ifEntry *mib, int ppa, char *name,
void *mdata),
void *mdata) {
dl_hp_ppa_req_t ppa_req;
dl_hp_ppa_ack_t *ppa_ack;
dl_hp_ppa_info_t *ppa_info;
mib_ifEntry mib;
void *buf;
int len, i, ofs;
char ifname[sizeof(ppa_info->dl_module_id_1) + 12];
if (!dlpi_attach(dlpi, DLPI_NO_PPA))
return 0;
ppa_req.dl_primitive = DL_HP_PPA_REQ;
if (!dlpi_req(dlpi, &ppa_req, sizeof(ppa_req),
DL_HP_PPA_ACK, (void **) &ppa_ack, &len))
return 0;
if (len < sizeof(dl_hp_ppa_ack_t)) {
ifstat_error("dlpi: short read for ppa ack");
return 0;
}
/* copy buffer since used by later calls */
if ((buf = malloc(len)) == NULL) {
perror("malloc");
return 0;
}
memcpy(buf, (void *) ppa_ack, len);
ppa_ack = buf;
/* browse interface list */
ofs = ppa_ack->dl_offset;
for(i = 0; i < ppa_ack->dl_count; i++) {
if (ofs < 0 || ofs + sizeof(dl_hp_ppa_info_t) > len) {
ifstat_error("dlpi: data returned by ppa ack exceeds data buffer");
free(buf);
return 0;
}
ppa_info = (dl_hp_ppa_info_t *) ((char *) ppa_ack + ofs);
if (dlpi_get_ifmib(dlpi, ppa_info->dl_ppa, &mib)) {
sprintf(ifname, "%s%d", ppa_info->dl_module_id_1, ppa_info->dl_instance_num);
if (!mapf(&mib, ppa_info->dl_ppa, ifname, mdata)) {
free(buf);
return 0;
}
}
ofs = ppa_ack->dl_offset + ppa_info->dl_next_offset;
}
free(buf);
return 1;
}
void pid_precalc()
{
timefactor = 0.0032f / looptime;
#ifdef PID_VOLTAGE_COMPENSATION
v_compensation = mapf ( vbattfilt , 3.00 , 4.00 , PID_VC_FACTOR , 1.00);
if( v_compensation > PID_VC_FACTOR) v_compensation = PID_VC_FACTOR;
if( v_compensation < 1.00f) v_compensation = 1.00;
#endif
}
void OnDraw()
{
glClear(GL_COLOR_BUFFER_BIT); //Clear The Screen And The Depth Buffer
for(int i=0; i<SIZE; i++)
{
glColor4f(gray[i], gray[i], gray[i], alpha[i]);
/*rect(mapf(i, 0, SIZE, clippingLeft, clippingRight),
clippingBottom,
mapf(i+1, 0, SIZE, clippingLeft, clippingRight), //width & Height can't be <0!!!!
abs(clippingBottom)+abs(clippingTop));*/
glRectf(mapf(i, 0, SIZE, clippingLeft, clippingRight),
clippingBottom,
mapf(i+1, 0, SIZE, clippingLeft, clippingRight),
clippingTop);
}
glFlush();
}
unsigned int CINFO3D_VISU::GetNrSegmentsCircle( int aDiameterBUI ) const
{
wxASSERT( aDiameterBUI > 0 );
unsigned int result = mapf( (float)aDiameterBUI,
(float)SEG_MIN_FACTOR_BIU, (float)SEG_MAX_FACTOR_BIU,
(float)MIN_SEG_PER_CIRCLE, (float)MAX_SEG_PER_CIRCLE );
wxASSERT( result > 1 );
return result;
}
unsigned int CINFO3D_VISU::GetNrSegmentsCircle( float aDiameter3DU ) const
{
wxASSERT( aDiameter3DU > 0.0f );
unsigned int result = mapf( aDiameter3DU,
m_calc_seg_min_factor3DU, m_calc_seg_max_factor3DU,
(float)MIN_SEG_PER_CIRCLE, (float)MAX_SEG_PER_CIRCLE );
wxASSERT( result > 1 );
return result;
}
static int snmp_map_ifs(struct ifstat_driver *driver,
int (*mapf)(struct ifstat_driver *driver, int nifaces, void *pdata),
void *pdata) {
struct snmp_driver_data *data = driver->data;
struct ifsnmp *ifsnmp = data->ifsnmp;
int ifaces, nifaces, index;
index = -1;
nifaces = ifaces = 0;
while((index = snmp_get_nextif(data->session, index)) >= 0) {
ifaces++;
ifsnmp[nifaces++].index = index;
if (nifaces >= data->num_ifsreqs) {
if(!mapf(driver, nifaces, pdata))
return 0;
nifaces = 0;
}
}
if (nifaces > 0)
return mapf(driver, nifaces, pdata);
return (ifaces != 0);
}
/*
* ai_api_map - call mapf for each element in indexer in order
* return: NO_ERROR if successful, error code otherwise
* indexer(in): VALUE_INDEXER
* mapf(in): map function
* arg(in): argument of the mapf
*/
static int
ai_api_map (VALUE_INDEXER * indexer, int (*mapf) (void *, int, VALUE_AREA *, API_VALUE *), void *arg)
{
ARRAY_INDEXER *ai = (ARRAY_INDEXER *) indexer;
int i, res;
assert (ai != NULL);
assert (mapf != NULL);
for (i = 0; i < ai->nvalue; i++)
{
res = mapf (arg, i, ai->vs_map[i], ai->values[i]);
if (res != NO_ERROR)
return res;
}
return NO_ERROR;
}
void CCape::Update( CCommand& commandIn )
{
if( time.HasElapsed( 100 ) )
{
// subtract the last reading:
total = total - readings[index];
// read from the sensor:
readings[index] = iout.Read();
delay( 1 );
// add the reading to the total:
total = total + readings[index];
// advance to the next position in the array:
index = index + 1;
// if we're at the end of the array...
if( index >= numReadings )
// ...wrap around to the beginning:
{
index = 0;
}
// calculate the average:
average = total / numReadings;
}
// send voltage and current
if( statustime.HasElapsed( 100 ) )
{
NDataManager::m_capeData.VOUT = mapf( vout.Read(), 0, 1023, 0, 50 );
NDataManager::m_capeData.IOUT = mapf( average, 0, 1023, 0, 5 ) + .4;
NDataManager::m_capeData.FMEM = FreeMemory();
NDataManager::m_capeData.ATMP = GetTemp();
NDataManager::m_capeData.UTIM = millis();
}
}
static int ioctl_map_ifs(int sd,
int (*mapf)(int sd, struct ifreq *ifr, void *data),
void *mdata) {
struct ifconf ifc;
struct ifreq *ifr;
int len, n, res = 0;
char *buf;
#ifdef SIOCGIFNUM
if (ioctl(sd, SIOCGIFNUM, &n) < 0) {
ifstat_perror("ioctl(SIOCGIFNUM):");
goto end;
}
n += 2;
#else
n = 256; /* bad bad bad... */
#endif
len = n * sizeof(struct ifreq);
if ((buf = malloc(len)) == NULL) {
ifstat_perror("malloc");
return 0;
}
ifc.ifc_buf = buf;
ifc.ifc_len = len;
if (ioctl(sd, SIOCGIFCONF, (char *)&ifc) < 0) {
ifstat_perror("ioctl(SIOCGIFCONF):");
goto end;
}
n = 0;
while (n < ifc.ifc_len) {
ifr = (struct ifreq *) (buf + n);
#ifdef HAVE_SOCKADDR_SA_LEN
n += sizeof(ifr->ifr_name) + ifr->ifr_addr.sa_len;
#else
n += sizeof(struct ifreq);
#endif
if (!mapf(sd, ifr, mdata))
goto end;
}
res = 1;
end:
free(buf);
return res;
}
static int ioctl_map_ifs(int sd,
int (*mapf)(int sd, struct ifreq *ifr, void *data),
void *mdata) {
struct if_nameindex *iflist, *cur;
struct ifreq ifr;
if ((iflist = if_nameindex()) == NULL) {
ifstat_perror("if_nameindex");
return 0;
}
for(cur = iflist; cur->if_index != 0 && cur->if_name != NULL; cur++) {
memcpy(ifr.ifr_name, cur->if_name, sizeof(ifr.ifr_name));
ifr.ifr_name[sizeof(ifr.ifr_name) - 1] = '\0';
if (!mapf(sd, &ifr, mdata))
return 0;
}
if_freenameindex(iflist);
return 1;
}
void World::mapStates(bool switchToContinuous) {
int x, y;
float state;
for (x = 0; x < _sizeX; x++) {
for (y = 0; y < _sizeY; y++) {
state = nodes[x][y].states[_index];
if (switchToContinuous)
state = unmapf(state, 0.0, _numStates - 1);
else
state = (float)((int)mapf(state, 0.0, _numStates - 1));
nodes[x][y].states[0] = state;
nodes[x][y].states[1] = state;
nodes[x][y].states[2] = state;
}
}
}
void control(void)
{
// hi rates
float ratemulti;
float ratemultiyaw;
float maxangle;
float anglerate;
if (aux[RATES])
{
ratemulti = HIRATEMULTI;
ratemultiyaw = HIRATEMULTIYAW;
maxangle = MAX_ANGLE_HI;
anglerate = LEVEL_MAX_RATE_HI;
}
else
{
ratemulti = 1.0f;
ratemultiyaw = 1.0f;
maxangle = MAX_ANGLE_LO;
anglerate = LEVEL_MAX_RATE_LO;
}
for (int i = 0; i < 3; i++)
{
#ifdef STOCK_TX_AUTOCENTER
rxcopy[i] = rx[i] - autocenter[i];
#else
rxcopy[i] = rx[i];
#endif
}
flip_sequencer();
if ( (!aux[STARTFLIP])&&auxchange[STARTFLIP] )
{
start_flip();
auxchange[STARTFLIP]= 0;
}
if (auxchange[HEADLESSMODE])
{
yawangle = 0;
}
if ((aux[HEADLESSMODE]) )
{
yawangle = yawangle + gyro[2] * looptime;
while (yawangle < -3.14159265f)
yawangle += 6.28318531f;
while (yawangle > 3.14159265f)
yawangle -= 6.28318531f;
float temp = rxcopy[0];
rxcopy[0] = rxcopy[0] * fastcos( yawangle) - rxcopy[1] * fastsin(yawangle );
rxcopy[1] = rxcopy[1] * fastcos( yawangle) + temp * fastsin(yawangle ) ;
}
else
{
yawangle = 0;
}
// check for acc calibration
int command = gestures2();
if (command)
{
if (command == 3)
{
gyro_cal(); // for flashing lights
acc_cal();
savecal();
// reset loop time
extern unsigned lastlooptime;
lastlooptime = gettime();
}
else
{
ledcommand = 1;
if (command == 2)
{
aux[CH_AUX1] = 1;
}
if (command == 1)
{
aux[CH_AUX1] = 0;
}
}
}
if ( controls_override)
{
for ( int i = 0 ; i < 3 ; i++)
{
rxcopy[i] = rx_override[i];
}
ratemulti = 1.0f;
maxangle = MAX_ANGLE_HI;
anglerate = LEVEL_MAX_RATE_HI;
}
imu_calc();
pid_precalc();
if ((aux[LEVELMODE]||level_override)&&!acro_override)
{ // level mode
angleerror[0] = rxcopy[0] * maxangle - attitude[0] + (float) TRIM_ROLL;
angleerror[1] = rxcopy[1] * maxangle - attitude[1] + (float) TRIM_PITCH;
error[0] = apid(0) * anglerate * DEGTORAD - gyro[0];
error[1] = apid(1) * anglerate * DEGTORAD - gyro[1];
}
else
{ // rate mode
error[0] = rxcopy[0] * MAX_RATE * DEGTORAD * ratemulti - gyro[0];
error[1] = rxcopy[1] * MAX_RATE * DEGTORAD * ratemulti - gyro[1];
// reduce angle Iterm towards zero
extern float aierror[3];
for (int i = 0; i <= 2; i++)
aierror[i] *= 0.8f;
}
error[2] = rxcopy[2] * MAX_RATEYAW * DEGTORAD * ratemultiyaw - gyro[2];
pid(0);
pid(1);
pid(2);
// map throttle so under 10% it is zero
float throttle = mapf(rx[3], 0, 1, -0.1, 1);
if (throttle < 0)
throttle = 0;
if (throttle > 1.0f)
throttle = 1.0f;
// turn motors off if throttle is off and pitch / roll sticks are centered
if (failsafe || (throttle < 0.001f && ( !ENABLESTIX || !onground_long || aux[LEVELMODE] || level_override || (fabsf(rx[0]) < (float) ENABLESTIX_TRESHOLD && fabsf(rx[1]) < (float) ENABLESTIX_TRESHOLD))))
{ // motors off
onground = 1;
if ( onground_long )
{
if ( gettime() - onground_long > 1000000)
{
onground_long = 0;
}
}
#ifdef MOTOR_BEEPS
extern void motorbeep( void);
motorbeep();
#endif
thrsum = 0;
for (int i = 0; i <= 3; i++)
{
pwm_set(i, 0);
}
// reset the overthrottle filter
lpf(&overthrottlefilt, 0.0f, 0.72f); // 50hz 1khz sample rate
#ifdef MOTOR_FILTER
// reset the motor filter
for (int i = 0; i <= 3; i++)
{
motorfilter(0, i);
}
#endif
#ifdef THROTTLE_TRANSIENT_COMPENSATION
// reset hpf filter;
throttlehpf(0);
#endif
#ifdef STOCK_TX_AUTOCENTER
for( int i = 0 ; i <3;i++)
{
if ( rx[i] == lastrx[i] )
{
consecutive[i]++;
}
else consecutive[i] = 0;
lastrx[i] = rx[i];
if ( consecutive[i] > 1000 && fabsf( rx[i]) < 0.1f )
{
autocenter[i] = rx[i];
}
}
#endif
}
else
{
// motors on - normal flight
onground_long = gettime();
#ifdef THROTTLE_TRANSIENT_COMPENSATION
throttle += 7.0f * throttlehpf(throttle);
if (throttle < 0)
throttle = 0;
if (throttle > 1.0f)
throttle = 1.0f;
#endif
// throttle angle compensation
#ifdef AUTO_THROTTLE
if (aux[LEVELMODE])
{
float autothrottle = fastcos(attitude[0] * DEGTORAD) * fastcos(attitude[1] * DEGTORAD);
float old_throttle = throttle;
if (autothrottle <= 0.5f)
autothrottle = 0.5f;
throttle = throttle / autothrottle;
// limit to 90%
if (old_throttle < 0.9f)
if (throttle > 0.9f)
throttle = 0.9f;
if (throttle > 1.0f)
throttle = 1.0f;
}
#endif
#ifdef LVC_PREVENT_RESET
extern float vbatt;
if (vbatt < (float) LVC_PREVENT_RESET_VOLTAGE) throttle = 0;
#endif
onground = 0;
float mix[4];
if ( controls_override)
{// change throttle in flip mode
throttle = rx_override[3];
}
#ifdef INVERT_YAW_PID
pidoutput[2] = -pidoutput[2];
#endif
mix[MOTOR_FR] = throttle - pidoutput[0] - pidoutput[1] + pidoutput[2]; // FR
mix[MOTOR_FL] = throttle + pidoutput[0] - pidoutput[1] - pidoutput[2]; // FL
mix[MOTOR_BR] = throttle - pidoutput[0] + pidoutput[1] - pidoutput[2]; // BR
mix[MOTOR_BL] = throttle + pidoutput[0] + pidoutput[1] + pidoutput[2]; // BL
#ifdef INVERT_YAW_PID
// we invert again cause it's used by the pid internally (for limit)
pidoutput[2] = -pidoutput[2];
#endif
#ifdef MIX_LOWER_THROTTLE
// limit reduction to this amount ( 0.0 - 1.0)
// 0.0 = no action
// 0.5 = reduce up to 1/2 throttle
//1.0 = reduce all the way to zero
#define MIX_THROTTLE_REDUCTION_MAX 0.5
float overthrottle = 0;
for (int i = 0; i < 4; i++)
{
if (mix[i] > overthrottle)
overthrottle = mix[i];
}
overthrottle -= 1.0f;
if (overthrottle > (float)MIX_THROTTLE_REDUCTION_MAX)
overthrottle = (float)MIX_THROTTLE_REDUCTION_MAX;
#ifdef MIX_THROTTLE_FILTER_LPF
if (overthrottle > overthrottlefilt)
lpf(&overthrottlefilt, overthrottle, 0.82); // 20hz 1khz sample rate
else
lpf(&overthrottlefilt, overthrottle, 0.72); // 50hz 1khz sample rate
#else
if (overthrottle > overthrottlefilt)
overthrottlefilt += 0.005f;
else
overthrottlefilt -= 0.01f;
#endif
if (overthrottlefilt > 0.5f)
overthrottlefilt = 0.5;
if (overthrottlefilt < -0.1f)
overthrottlefilt = -0.1;
overthrottle = overthrottlefilt;
if (overthrottle < 0.0f)
overthrottle = 0.0f;
if (overthrottle > 0)
{ // exceeding max motor thrust
// prevent too much throttle reduction
if (overthrottle > (float)MIX_THROTTLE_REDUCTION_MAX)
overthrottle = (float)MIX_THROTTLE_REDUCTION_MAX;
// reduce by a percentage only, so we get an inbetween performance
overthrottle *= ((float)MIX_THROTTLE_REDUCTION_PERCENT / 100.0f);
for (int i = 0; i < 4; i++)
{
mix[i] -= overthrottle;
}
}
#endif
#ifdef MOTOR_FILTER
for (int i = 0; i < 4; i++)
{
mix[i] = motorfilter(mix[i], i);
}
#endif
#ifdef CLIP_FF
float clip_ff(float motorin, int number);
for (int i = 0; i < 4; i++)
{
mix[i] = clip_ff(mix[i], i);
}
#endif
for (int i = 0; i < 4; i++)
{
float test = motormap(mix[i]);
#ifdef MOTORS_TO_THROTTLE
test = throttle;
// flash leds in valid throttle range
ledcommand = 1;
// for battery estimation
mix[i] = throttle;
#warning "MOTORS TEST MODE"
#endif
#ifdef MOTOR_MIN_ENABLE
if (test < (float) MOTOR_MIN_VALUE)
{
test = (float) MOTOR_MIN_VALUE;
}
#endif
#ifdef MOTOR_MAX_ENABLE
if (test > (float) MOTOR_MAX_VALUE)
{
test = (float) MOTOR_MAX_VALUE;
}
#endif
#ifndef NOMOTORS
//normal mode
pwm_set( i , test );
#else
#warning "NO MOTORS"
#endif
}
thrsum = 0;
for (int i = 0; i < 4; i++)
{
if (mix[i] < 0)
mix[i] = 0;
if (mix[i] > 1)
mix[i] = 1;
thrsum += mix[i];
}
thrsum = thrsum / 4;
} // end motors on
}
void readTemp(){
float analogTempRead = analogRead(temppin);
float volt = mapf(analogTempRead*1.0,0,1024,0,5.0); // change 4: 1024.0, otherwise will calc integer value!!
celsiusTempRead = (volt-.4)*51.28;
}
void control( void)
{
// hi rates
float ratemulti;
float ratemultiyaw;
float maxangle;
float anglerate;
if ( aux[RATES] )
{
ratemulti = HIRATEMULTI;
ratemultiyaw = HIRATEMULTIYAW;
maxangle = MAX_ANGLE_HI;
anglerate = LEVEL_MAX_RATE_HI;
}
else
{
ratemulti = 1.0f;
ratemultiyaw = 1.0f;
maxangle = MAX_ANGLE_LO;
anglerate = LEVEL_MAX_RATE_LO;
}
for ( int i = 0 ; i < 3; i++)
{
rxcopy[i]= rx[i];
}
if ( auxchange[HEADLESSMODE] )
{
yawangle = 0;
}
if ( (aux[HEADLESSMODE] )&&!aux[LEVELMODE] )
{
yawangle = yawangle + gyro[2]*looptime;
float temp = rxcopy[0];
rxcopy[0] = rxcopy[0] * cosf( yawangle) - rxcopy[1] * sinf(yawangle );
rxcopy[1] = rxcopy[1] * cosf( yawangle) + temp * sinf(yawangle ) ;
}
else
{
yawangle = 0;
}
// check for acc calibration
int command = gestures2();
if ( command )
{
if ( command == 3 )
{
gyro_cal(); // for flashing lights
acc_cal();
savecal();
// reset loop time
extern unsigned lastlooptime;
lastlooptime = gettime();
}
else
{
ledcommand = 1;
if ( command == 2 )
{
aux[CH_AUX1]= 1;
}
if ( command == 1 )
{
aux[CH_AUX1]= 0;
}
}
}
imu_calc();
pid_precalc();
if ( aux[LEVELMODE] )
{ // level mode
angleerror[0] = rxcopy[0] * maxangle - attitude[0];
angleerror[1] = rxcopy[1] * maxangle - attitude[1];
error[0] = apid(0) * anglerate * DEGTORAD - gyro[0];
error[1] = apid(1) * anglerate * DEGTORAD - gyro[1];
}
else
{ // rate mode
error[0] = rxcopy[0] * MAX_RATE * DEGTORAD * ratemulti - gyro[0];
error[1] = rxcopy[1] * MAX_RATE * DEGTORAD * ratemulti - gyro[1];
// reduce angle Iterm towards zero
extern float aierror[3];
for ( int i = 0 ; i <= 2 ; i++) aierror[i] *= 0.8f;
}
error[2] = rxcopy[2] * MAX_RATEYAW * DEGTORAD * ratemultiyaw - gyro[2];
pid(0);
pid(1);
pid(2);
// map throttle so under 10% it is zero
float throttle = mapf(rx[3], 0 , 1 , -0.1 , 1 );
if ( throttle < 0 ) throttle = 0;
if ( throttle > 1.0f ) throttle = 1.0f;
// turn motors off if throttle is off and pitch / roll sticks are centered
if ( failsafe || (throttle < 0.001f && (!ENABLESTIX|| (fabs(rx[0]) < 0.5f && fabs(rx[1]) < 0.5f ) ) ) )
{ // motors off
onground = 1;
thrsum = 0;
for ( int i = 0 ; i <= 3 ; i++)
{
pwm_set( i , 0 );
}
// reset the overthrottle filter
lpf( &overthrottlefilt, 0.0f , 0.72f); // 50hz 1khz sample rate
#ifdef MOTOR_FILTER
// reset the motor filter
for ( int i = 0 ; i <= 3 ; i++)
{
motorfilter( 0 , i);
}
#endif
#ifdef THROTTLE_TRANSIENT_COMPENSATION
// reset hpf filter;
throttlehpf( 0 );
#endif
}
else
{
#ifdef THROTTLE_TRANSIENT_COMPENSATION
throttle += 7.0f*throttlehpf( throttle );
if ( throttle < 0 ) throttle = 0;
if ( throttle > 1.0f ) throttle = 1.0f;
#endif
// throttle angle compensation
#ifdef AUTO_THROTTLE
if ( aux[LEVELMODE]||AUTO_THROTTLE_ACRO_MODE )
{
float autothrottle = fastcos( attitude[0] * DEGTORAD ) * fastcos( attitude[1] * DEGTORAD );
float old_throttle = throttle;
if ( autothrottle <= 0.5f ) autothrottle = 0.5f;
throttle = throttle / autothrottle;
// limit to 90%
if ( old_throttle < 0.9f ) if ( throttle > 0.9f ) throttle = 0.9f;
if ( throttle > 1.0f ) throttle = 1.0f;
}
#endif
onground = 0;
float mix[4];
mix[MOTOR_FR] = throttle - pidoutput[0] - pidoutput[1] + pidoutput[2]; // FR
mix[MOTOR_FL] = throttle + pidoutput[0] - pidoutput[1] - pidoutput[2]; // FL
mix[MOTOR_BR] = throttle - pidoutput[0] + pidoutput[1] - pidoutput[2]; // BR
mix[MOTOR_BL] = throttle + pidoutput[0] + pidoutput[1] + pidoutput[2]; // BL
#ifdef MIX_LOWER_THROTTLE
// limit reduction to this amount ( 0.0 - 1.0)
// 0.0 = no action
// 0.5 = reduce up to 1/2 throttle
//1.0 = reduce all the way to zero
#define MIX_THROTTLE_REDUCTION_MAX 0.5
float overthrottle = 0;
for ( int i = 0 ; i < 3 ; i++)
{
if ( mix[i] > overthrottle ) overthrottle = mix[i];
}
overthrottle -=1.0f;
if ( overthrottle > (float) MIX_THROTTLE_REDUCTION_MAX ) overthrottle = (float) MIX_THROTTLE_REDUCTION_MAX;
#ifdef MIX_THROTTLE_FILTER_LPF
if (overthrottle > overthrottlefilt) lpf ( &overthrottlefilt, overthrottle , 0.82); // 20hz 1khz sample rate
else lpf ( &overthrottlefilt, overthrottle , 0.72); // 50hz 1khz sample rate
#else
if (overthrottle > overthrottlefilt) overthrottlefilt += 0.005f;
else overthrottlefilt -= 0.01f;
#endif
if (overthrottlefilt > 0.5f) overthrottlefilt = 0.5;
if (overthrottlefilt < -0.1f) overthrottlefilt = -0.1;
overthrottle = overthrottlefilt;
if ( overthrottle < 0.0f ) overthrottle = 0.0f;
if ( overthrottle > 0 )
{ // exceeding max motor thrust
// prevent too much throttle reduction
if ( overthrottle > (float) MIX_THROTTLE_REDUCTION_MAX ) overthrottle = (float) MIX_THROTTLE_REDUCTION_MAX;
// reduce by a percentage only, so we get an inbetween performance
overthrottle *= ((float) MIX_THROTTLE_REDUCTION_PERCENT / 100.0f);
for ( int i = 0 ; i < 3 ; i++)
{
mix[i] -=overthrottle;
}
}
#endif
#ifdef MOTOR_FILTER
for ( int i = 0 ; i <= 3 ; i++)
{
mix[i] = motorfilter( mix[i] , i);
}
#endif
#ifdef CLIP_FF
float clip_ff( float motorin ,int number);
for ( int i = 0 ; i <= 3 ; i++)
{
mix[i] = clip_ff( mix[i] , i);
}
#endif
for ( int i = 0 ; i <= 3 ; i++)
{
float test = motormap( mix[i] );
#ifndef NOMOTORS
pwm_set( i , ( test ) );
#else
#warning "NO MOTORS"
#endif
}
thrsum = 0;
for ( int i = 0 ; i <= 3 ; i++)
{
if ( mix[i] < 0 ) mix[i] = 0;
if ( mix[i] > 1 ) mix[i] = 1;
thrsum+= mix[i];
}
thrsum = thrsum / 4;
}// end motors on
}
void C3D_RENDER_OGL_LEGACY::reload()
{
m_reloadRequested = false;
ogl_free_all_display_lists();
COBJECT2D_STATS::Instance().ResetStats();
m_settings.InitSettings();
SFVEC3F camera_pos = m_settings.GetBoardCenter3DU();
m_settings.CameraGet().SetBoardLookAtPos( camera_pos );
// Create Board
// /////////////////////////////////////////////////////////////////////////
printf("Create board...\n");
CCONTAINER2D boardContainer;
Convert_shape_line_polygon_to_triangles( m_settings.GetBoardPoly(),
boardContainer,
m_settings.BiuTo3Dunits(),
(const BOARD_ITEM &)*m_settings.GetBoard() );
const LIST_OBJECT2D listBoardObject2d = boardContainer.GetList();
if( listBoardObject2d.size() > 0 )
{
/*
float layer_z_top = m_settings.GetLayerBottomZpos3DU( F_Cu );
float layer_z_bot = m_settings.GetLayerBottomZpos3DU( B_Cu );
*/
float layer_z_top = m_settings.GetLayerBottomZpos3DU( B_Mask );
float layer_z_bot = m_settings.GetLayerTopZpos3DU( B_Mask );
CLAYER_TRIANGLES *layerTriangles = new CLAYER_TRIANGLES( listBoardObject2d.size() );
for( LIST_OBJECT2D::const_iterator itemOnLayer = listBoardObject2d.begin();
itemOnLayer != listBoardObject2d.end();
itemOnLayer++ )
{
const COBJECT2D *object2d_A = static_cast<const COBJECT2D *>(*itemOnLayer);
wxASSERT( object2d_A->GetObjectType() == OBJ2D_TRIANGLE );
const CTRIANGLE2D *tri = (const CTRIANGLE2D *)object2d_A;
const SFVEC2F &v1 = tri->GetP1();
const SFVEC2F &v2 = tri->GetP2();
const SFVEC2F &v3 = tri->GetP3();
add_triangle_top_bot( layerTriangles, v1, v2, v3, layer_z_top, layer_z_bot );
}
const SHAPE_POLY_SET boardPoly = m_settings.GetBoardPoly();
SHAPE_POLY_SET boardPolyCopy = boardPoly;
boardPolyCopy.Simplify( SHAPE_POLY_SET::PM_FAST );
if( boardPolyCopy.OutlineCount() == 1 )
{
const SHAPE_LINE_CHAIN& outlinePath = boardPolyCopy.COutline( 0 );
std::vector< SFVEC2F > contournPoints;
contournPoints.clear();
contournPoints.reserve( outlinePath.PointCount() + 2 );
for( unsigned int i = 0; i < (unsigned int)outlinePath.PointCount(); ++i )
{
const VECTOR2I& v = outlinePath.CPoint( i );
contournPoints.push_back( SFVEC2F( v.x * m_settings.BiuTo3Dunits(),
-v.y * m_settings.BiuTo3Dunits() ) );
}
contournPoints.push_back( contournPoints[0] );
if( contournPoints.size() > 4 )
{
// Calculate normals of each segment of the contourn
std::vector< SFVEC2F > contournNormals;
contournNormals.clear();
contournNormals.reserve( contournPoints.size() );
for( unsigned int i = 0; i < ( contournPoints.size() - 1 ); ++i )
{
const SFVEC2F &v0 = contournPoints[i + 0];
const SFVEC2F &v1 = contournPoints[i + 1];
SFVEC2F n = glm::normalize( v1 - v0 );
contournNormals.push_back( SFVEC2F( -n.y, n.x ) );
}
SFVEC2F lastNormal = contournNormals[contournPoints.size() - 2];
for( unsigned int i = 0; i < ( contournPoints.size() - 1 ); ++i )
{
const SFVEC2F &v0 = contournPoints[i + 0];
const SFVEC2F &v1 = contournPoints[i + 1];
layerTriangles->m_layer_middle_contourns_quads->AddQuad( SFVEC3F( v0.x, v0.y, NextFloatUp( layer_z_top ) ),
SFVEC3F( v1.x, v1.y, NextFloatUp( layer_z_top ) ),
SFVEC3F( v1.x, v1.y, NextFloatDown( layer_z_bot ) ),
SFVEC3F( v0.x, v0.y, NextFloatDown( layer_z_bot ) ) );
SFVEC2F n0 = contournNormals[i];
if( glm::dot( n0, lastNormal ) > 0.5f )
n0 += lastNormal;
else
n0 += contournNormals[i];
const SFVEC2F &nextNormal = contournNormals[ (i + 1) % (contournPoints.size() - 1) ];
SFVEC2F n1 = contournNormals[i];
if( glm::dot( n1, nextNormal ) > 0.5f )
n1 += nextNormal;
else
n1 += contournNormals[i];
n0 = glm::normalize( n0 );
n1 = glm::normalize( n1 );
const SFVEC3F n3d0 = SFVEC3F( n0.x, n0.y, 0.0f );
const SFVEC3F n3d1 = SFVEC3F( n1.x, n1.y, 0.0f );
layerTriangles->m_layer_middle_contourns_quads->AddNormal( n3d0, n3d1, n3d1, n3d0 );
lastNormal = contournNormals[i];
/*
const SFVEC2F n0 = glm::normalize( v0 - center );
const SFVEC2F n1 = glm::normalize( v1 - center );
const SFVEC3F n0z = SFVEC3F( n0.x, n0.y, 0.0f );
const SFVEC3F n1z = SFVEC3F( n1.x, n1.y, 0.0f );
layerTriangles->m_layer_middle_contourns_quads->AddNormal( n0z, n1z, n1z, n0z );*/
}
}
contournPoints.clear();
}
m_ogl_disp_list_board = new CLAYERS_OGL_DISP_LISTS( *layerTriangles, m_ogl_circle_texture, SFVEC3F(0.65f,0.55f,0.05f) );
delete layerTriangles;
}
float calc_sides_min_factor = (float)( 10.0 * IU_PER_MILS * m_settings.BiuTo3Dunits() );
float calc_sides_max_factor = (float)( 1000.0 * IU_PER_MILS * m_settings.BiuTo3Dunits() );
// Add layers maps (except B_Mask and F_Mask)
// /////////////////////////////////////////////////////////////////////////
printf("Add layers maps...\n");
for( MAP_CONTAINER_2D::const_iterator it = m_settings.GetMapLayers().begin();
it != m_settings.GetMapLayers().end(); it++ )
{
LAYER_ID layer_id = static_cast<LAYER_ID>(it->first);
if( !m_settings.Is3DLayerEnabled( layer_id ) )
continue;
const CBVHCONTAINER2D *container2d = static_cast<const CBVHCONTAINER2D *>(it->second);
const LIST_OBJECT2D listObject2d = container2d->GetList();
if( listObject2d.size() == 0 )
continue;
//CMATERIAL *materialLayer = &m_materials.m_SilkS;
SFVEC3F layerColor = SFVEC3F( 0.3f, 0.4f, 0.5f );
float layer_z_bot = m_settings.GetLayerBottomZpos3DU( layer_id );
float layer_z_top = m_settings.GetLayerTopZpos3DU( layer_id );
if( layer_z_top < layer_z_bot )
{
float tmpFloat = layer_z_bot;
layer_z_bot = layer_z_top;
layer_z_top = tmpFloat;
}
layer_z_bot -= m_settings.GetNonCopperLayerThickness3DU();
layer_z_top += m_settings.GetNonCopperLayerThickness3DU();
if( m_settings.GetFlag( FL_USE_REALISTIC_MODE ) )
{
switch( layer_id )
{
case B_Adhes:
case F_Adhes:
break;
case B_Paste:
case F_Paste:
// materialLayer = &m_materials.m_Paste;
break;
case B_SilkS:
case F_SilkS:
// materialLayer = &m_materials.m_SilkS;
// layerColor = g_silkscreenColor;
break;
case Dwgs_User:
case Cmts_User:
case Eco1_User:
case Eco2_User:
case Edge_Cuts:
case Margin:
break;
case B_CrtYd:
case F_CrtYd:
break;
case B_Fab:
case F_Fab:
break;
default:
//materialLayer = &m_materials.m_Copper;
//layerColor = g_copperColor;
break;
}
}
else
{
layerColor = m_settings.GetLayerColor( layer_id );
}
// Calculate an estiation for then nr of triangles based on the nr of objects
unsigned int nrTrianglesEstimation = listObject2d.size() * 8;
CLAYER_TRIANGLES *layerTriangles = new CLAYER_TRIANGLES( nrTrianglesEstimation );
m_triangles[layer_id] = layerTriangles;
for( LIST_OBJECT2D::const_iterator itemOnLayer = listObject2d.begin();
itemOnLayer != listObject2d.end(); itemOnLayer++ )
{
const COBJECT2D *object2d_A = static_cast<const COBJECT2D *>(*itemOnLayer);
switch( object2d_A->GetObjectType() )
{
case OBJ2D_FILLED_CIRCLE:
{
const CFILLEDCIRCLE2D *filledCircle = (const CFILLEDCIRCLE2D *)object2d_A;
const SFVEC2F ¢er = filledCircle->GetCenter();
float radius = filledCircle->GetRadius() * 2.0f; // Double because the render triangle
float radiusSquared = radius * radius;
const float f = (sqrtf(2.0f) / 2.0f) * radius * 0.9;// * texture_factor;
layerTriangles->m_layer_top_segment_ends->AddTriangle( SFVEC3F( center.x + f, center.y, layer_z_top ),
SFVEC3F( center.x - f, center.y, layer_z_top ),
SFVEC3F( center.x,
center.y - f, layer_z_top ) );
layerTriangles->m_layer_top_segment_ends->AddTriangle( SFVEC3F( center.x - f, center.y, layer_z_top ),
SFVEC3F( center.x + f, center.y, layer_z_top ),
SFVEC3F( center.x,
center.y + f, layer_z_top ) );
layerTriangles->m_layer_bot_segment_ends->AddTriangle( SFVEC3F( center.x - f, center.y, layer_z_bot ),
SFVEC3F( center.x + f, center.y, layer_z_bot ),
SFVEC3F( center.x,
center.y - f, layer_z_bot ) );
layerTriangles->m_layer_bot_segment_ends->AddTriangle( SFVEC3F( center.x + f, center.y, layer_z_bot ),
SFVEC3F( center.x - f, center.y, layer_z_bot ),
SFVEC3F( center.x,
center.y + f, layer_z_bot ) );
unsigned int nr_sides_per_circle = (unsigned int)mapf( radiusSquared,
calc_sides_min_factor, calc_sides_max_factor,
24.0f, 256.0f );
wxASSERT( nr_sides_per_circle >= 24 );
// Normal radius for the circle
radius = filledCircle->GetRadius();
std::vector< SFVEC2F > contournPoints;
contournPoints.clear();
contournPoints.reserve( nr_sides_per_circle + 2 );
int delta = 3600 / nr_sides_per_circle;
for( int ii = 0; ii < 3600; ii += delta )
{
const SFVEC2F rotatedDir = glm::rotate( SFVEC2F( 0.0f, 1.0f ), (float)ii * 2.0f * 3.14f / 3600.0f );
contournPoints.push_back( SFVEC2F( center.x - rotatedDir.y * radius, center.y + rotatedDir.x * radius ) );
}
contournPoints.push_back( contournPoints[0] );
if( contournPoints.size() > 1 )
{
for( unsigned int i = 0; i < ( contournPoints.size() - 1 ); ++i )
{
const SFVEC2F &v0 = contournPoints[i + 0];
const SFVEC2F &v1 = contournPoints[i + 1];
layerTriangles->m_layer_middle_contourns_quads->AddQuad( SFVEC3F( v0.x, v0.y, NextFloatUp( layer_z_bot ) ),
SFVEC3F( v1.x, v1.y, NextFloatUp( layer_z_bot ) ),
SFVEC3F( v1.x, v1.y, NextFloatDown( layer_z_top ) ),
SFVEC3F( v0.x, v0.y, NextFloatDown( layer_z_top ) ) );
const SFVEC2F n0 = glm::normalize( v0 - center );
const SFVEC2F n1 = glm::normalize( v1 - center );
const SFVEC3F n0z = SFVEC3F( n0.x, n0.y, 0.0f );
const SFVEC3F n1z = SFVEC3F( n1.x, n1.y, 0.0f );
layerTriangles->m_layer_middle_contourns_quads->AddNormal( n0z, n1z, n1z, n0z );
}
}
contournPoints.clear();
}
break;
case OBJ2D_DUMMYBLOCK:
{
}
break;
case OBJ2D_POLYGON4PT:
{
const CPOLYGON4PTS2D *poly = (const CPOLYGON4PTS2D *)object2d_A;
const SFVEC2F &v0 = poly->GetV0();
const SFVEC2F &v1 = poly->GetV1();
const SFVEC2F &v2 = poly->GetV2();
const SFVEC2F &v3 = poly->GetV3();
add_triangle_top_bot( layerTriangles, v0, v2, v1, layer_z_top, layer_z_bot );
add_triangle_top_bot( layerTriangles, v2, v0, v3, layer_z_top, layer_z_bot );
const SFVEC2F &n0 = poly->GetN0();
const SFVEC2F &n1 = poly->GetN1();
const SFVEC2F &n2 = poly->GetN2();
const SFVEC2F &n3 = poly->GetN3();
const SFVEC3F n3d0 = SFVEC3F(-n0.y, n0.x, 0.0f );
const SFVEC3F n3d1 = SFVEC3F(-n1.y, n1.x, 0.0f );
const SFVEC3F n3d2 = SFVEC3F(-n2.y, n2.x, 0.0f );
const SFVEC3F n3d3 = SFVEC3F(-n3.y, n3.x, 0.0f );
layerTriangles->m_layer_middle_contourns_quads->AddQuad( SFVEC3F( v0.x, v0.y, layer_z_bot ),
SFVEC3F( v1.x, v1.y, layer_z_bot ),
SFVEC3F( v1.x, v1.y, layer_z_top ),
SFVEC3F( v0.x, v0.y, layer_z_top ) );
layerTriangles->m_layer_middle_contourns_quads->AddNormal( n3d0, n3d0, n3d0, n3d0 );
layerTriangles->m_layer_middle_contourns_quads->AddQuad( SFVEC3F( v2.x, v2.y, layer_z_top ),
SFVEC3F( v1.x, v1.y, layer_z_top ),
SFVEC3F( v1.x, v1.y, layer_z_bot ),
SFVEC3F( v2.x, v2.y, layer_z_bot ) );
layerTriangles->m_layer_middle_contourns_quads->AddNormal( n3d1, n3d1, n3d1, n3d1 );
layerTriangles->m_layer_middle_contourns_quads->AddQuad( SFVEC3F( v3.x, v3.y, layer_z_top ),
SFVEC3F( v2.x, v2.y, layer_z_top ),
SFVEC3F( v2.x, v2.y, layer_z_bot ),
SFVEC3F( v3.x, v3.y, layer_z_bot ) );
layerTriangles->m_layer_middle_contourns_quads->AddNormal( n3d2, n3d2, n3d2, n3d2 );
layerTriangles->m_layer_middle_contourns_quads->AddQuad( SFVEC3F( v0.x, v0.y, layer_z_top ),
SFVEC3F( v3.x, v3.y, layer_z_top ),
SFVEC3F( v3.x, v3.y, layer_z_bot ),
SFVEC3F( v0.x, v0.y, layer_z_bot ) );
layerTriangles->m_layer_middle_contourns_quads->AddNormal( n3d3, n3d3, n3d3, n3d3 );
}
break;
case OBJ2D_RING:
{
const CRING2D *ring = (const CRING2D *)object2d_A;
const SFVEC2F ¢er = ring->GetCenter();
float inner = ring->GetInnerRadius();
float outer = ring->GetOuterRadius();
unsigned int nr_sides_per_circle = (unsigned int)mapf( outer,
calc_sides_min_factor, calc_sides_max_factor,
24.0f, 256.0f );
wxASSERT( nr_sides_per_circle >= 24 );
std::vector< SFVEC2F > innerContour;
std::vector< SFVEC2F > outerContour;
innerContour.clear();
innerContour.reserve( nr_sides_per_circle + 2 );
outerContour.clear();
outerContour.reserve( nr_sides_per_circle + 2 );
int delta = 3600 / nr_sides_per_circle;
for( int ii = 0; ii < 3600; ii += delta )
{
const SFVEC2F rotatedDir = glm::rotate( SFVEC2F( 0.0f, 1.0f), (float) ii * 2.0f * 3.14f / 3600.0f );
innerContour.push_back( SFVEC2F( center.x - rotatedDir.y * inner, center.y + rotatedDir.x * inner ) );
outerContour.push_back( SFVEC2F( center.x - rotatedDir.y * outer, center.y + rotatedDir.x * outer ) );
}
innerContour.push_back( innerContour[0] );
outerContour.push_back( outerContour[0] );
wxASSERT( innerContour.size() == outerContour.size() );
for( unsigned int i = 0; i < ( innerContour.size() - 1 ); ++i )
{
const SFVEC2F &vi0 = innerContour[i + 0];
const SFVEC2F &vi1 = innerContour[i + 1];
const SFVEC2F &vo0 = outerContour[i + 0];
const SFVEC2F &vo1 = outerContour[i + 1];
layerTriangles->m_layer_top_triangles->AddQuad( SFVEC3F( vi1.x, vi1.y, layer_z_top ),
SFVEC3F( vi0.x, vi0.y, layer_z_top ),
SFVEC3F( vo0.x, vo0.y, layer_z_top ),
SFVEC3F( vo1.x, vo1.y, layer_z_top ) );
layerTriangles->m_layer_bot_triangles->AddQuad( SFVEC3F( vi1.x, vi1.y, layer_z_bot ),
SFVEC3F( vo1.x, vo1.y, layer_z_bot ),
SFVEC3F( vo0.x, vo0.y, layer_z_bot ),
SFVEC3F( vi0.x, vi0.y, layer_z_bot ) );
}
for( unsigned int i = 0; i < ( innerContour.size() - 1 ); ++i )
{
const SFVEC2F &v0 = innerContour[i + 0];
const SFVEC2F &v1 = innerContour[i + 1];
layerTriangles->m_layer_middle_contourns_quads->AddQuad( SFVEC3F( v1.x, v1.y, NextFloatUp( layer_z_bot ) ),
SFVEC3F( v0.x, v0.y, NextFloatUp( layer_z_bot ) ),
SFVEC3F( v0.x, v0.y, NextFloatDown( layer_z_top ) ),
SFVEC3F( v1.x, v1.y, NextFloatDown( layer_z_top ) ) );
const SFVEC2F n0 = glm::normalize( v0 - center );
const SFVEC2F n1 = glm::normalize( v1 - center );
const SFVEC3F n0z = SFVEC3F( n0.x, n0.y, 0.0f );
const SFVEC3F n1z = SFVEC3F( n1.x, n1.y, 0.0f );
layerTriangles->m_layer_middle_contourns_quads->AddNormal( n0z, n1z, n1z, n0z );
}
for( unsigned int i = 0; i < ( outerContour.size() - 1 ); ++i )
{
const SFVEC2F &v0 = outerContour[i + 0];
const SFVEC2F &v1 = outerContour[i + 1];
layerTriangles->m_layer_middle_contourns_quads->AddQuad( SFVEC3F( v0.x, v0.y, NextFloatUp( layer_z_bot ) ),
SFVEC3F( v1.x, v1.y, NextFloatUp( layer_z_bot ) ),
SFVEC3F( v1.x, v1.y, NextFloatDown( layer_z_top ) ),
SFVEC3F( v0.x, v0.y, NextFloatDown( layer_z_top ) ) );
const SFVEC2F n0 = glm::normalize( v0 - center );
const SFVEC2F n1 = glm::normalize( v1 - center );
const SFVEC3F n0z = SFVEC3F( n0.x, n0.y, 0.0f );
const SFVEC3F n1z = SFVEC3F( n1.x, n1.y, 0.0f );
layerTriangles->m_layer_middle_contourns_quads->AddNormal( n0z, n1z, n1z, n0z );
}
}
break;
case OBJ2D_TRIANGLE:
{
const CTRIANGLE2D *tri = (const CTRIANGLE2D *)object2d_A;
const SFVEC2F &v1 = tri->GetP1();
const SFVEC2F &v2 = tri->GetP2();
const SFVEC2F &v3 = tri->GetP3();
add_triangle_top_bot( layerTriangles, v1, v2, v3, layer_z_top, layer_z_bot );
}
break;
case OBJ2D_ROUNDSEG:
{
const CROUNDSEGMENT2D &roundSeg = (const CROUNDSEGMENT2D &) *object2d_A;
unsigned int nr_sides_per_circle = (unsigned int)mapf( roundSeg.GetWidth(),
calc_sides_min_factor, calc_sides_max_factor,
24.0f, 256.0f );
wxASSERT( nr_sides_per_circle >= 24 );
SFVEC2F leftStart = roundSeg.GetLeftStar();
SFVEC2F leftEnd = roundSeg.GetLeftEnd();
SFVEC2F leftDir = roundSeg.GetLeftDir();
SFVEC2F rightStart = roundSeg.GetRightStar();
SFVEC2F rightEnd = roundSeg.GetRightEnd();
SFVEC2F rightDir = roundSeg.GetRightDir();
float radius = roundSeg.GetRadius();
SFVEC2F start = roundSeg.GetStart();
SFVEC2F end = roundSeg.GetEnd();
float texture_factor = (12.0f/(float)SIZE_OF_CIRCLE_TEXTURE) + 1.0f;
float texture_factorF= ( 4.0f/(float)SIZE_OF_CIRCLE_TEXTURE) + 1.0f;
const float radius_of_the_square = sqrtf( roundSeg.GetRadiusSquared() * 2.0f );
const float radius_triangle_factor = (radius_of_the_square - radius) / radius;
const SFVEC2F factorS = SFVEC2F( -rightDir.y * radius * radius_triangle_factor, rightDir.x * radius * radius_triangle_factor );
const SFVEC2F factorE = SFVEC2F( -leftDir.y * radius * radius_triangle_factor, leftDir.x * radius * radius_triangle_factor );
// Top end segment triangles
layerTriangles->m_layer_top_segment_ends->AddTriangle( SFVEC3F( rightEnd.x + texture_factor * factorS.x, rightEnd.y + texture_factor * factorS.y, layer_z_top ),
SFVEC3F( leftStart.x + texture_factor * factorE.x, leftStart.y + texture_factor * factorE.y, layer_z_top ),
SFVEC3F( start.x - texture_factorF * leftDir.x * radius * sqrtf(2.0f),
start.y - texture_factorF * leftDir.y * radius * sqrtf(2.0f), layer_z_top ) );
layerTriangles->m_layer_top_segment_ends->AddTriangle( SFVEC3F( leftEnd.x + texture_factor * factorE.x, leftEnd.y + texture_factor * factorE.y, layer_z_top ),
SFVEC3F( rightStart.x + texture_factor * factorS.x, rightStart.y + texture_factor * factorS.y, layer_z_top ),
SFVEC3F( end.x - texture_factorF * rightDir.x * radius * sqrtf(2.0f),
end.y - texture_factorF * rightDir.y * radius * sqrtf(2.0f), layer_z_top ) );
// Bot end segment triangles
layerTriangles->m_layer_bot_segment_ends->AddTriangle( SFVEC3F( leftStart.x + texture_factor * factorE.x, leftStart.y + texture_factor * factorE.y, layer_z_bot ),
SFVEC3F( rightEnd.x + texture_factor * factorS.x, rightEnd.y + texture_factor * factorS.y, layer_z_bot ),
SFVEC3F( start.x - texture_factorF * leftDir.x * radius * sqrtf(2.0f),
start.y - texture_factorF * leftDir.y * radius * sqrtf(2.0f), layer_z_bot ) );
layerTriangles->m_layer_bot_segment_ends->AddTriangle( SFVEC3F( rightStart.x + texture_factor * factorS.x, rightStart.y + texture_factor * factorS.y, layer_z_bot ),
SFVEC3F( leftEnd.x + texture_factor * factorE.x, leftEnd.y + texture_factor * factorE.y, layer_z_bot ),
SFVEC3F( end.x - texture_factorF * rightDir.x * radius * sqrtf(2.0f),
end.y - texture_factorF * rightDir.y * radius * sqrtf(2.0f), layer_z_bot ) );
// Segment top and bot planes
layerTriangles->m_layer_top_triangles->AddQuad( SFVEC3F( rightEnd.x, rightEnd.y, layer_z_top ),
SFVEC3F( rightStart.x, rightStart.y, layer_z_top ),
SFVEC3F( leftEnd.x, leftEnd.y, layer_z_top ),
SFVEC3F( leftStart.x, leftStart.y, layer_z_top ) );
layerTriangles->m_layer_bot_triangles->AddQuad( SFVEC3F( rightEnd.x, rightEnd.y, layer_z_bot ),
SFVEC3F( leftStart.x, leftStart.y, layer_z_bot ),
SFVEC3F( leftEnd.x, leftEnd.y, layer_z_bot ),
SFVEC3F( rightStart.x, rightStart.y, layer_z_bot ) );
// Middle contourns (two sides of the segment)
layerTriangles->m_layer_middle_contourns_quads->AddQuad( SFVEC3F( leftStart.x, leftStart.y, layer_z_top ),
SFVEC3F( leftEnd.x, leftEnd.y, layer_z_top ),
SFVEC3F( leftEnd.x, leftEnd.y, layer_z_bot ),
SFVEC3F( leftStart.x, leftStart.y, layer_z_bot ) );
const SFVEC3F leftNormal = SFVEC3F( -leftDir.y, leftDir.x, 0.0f );
layerTriangles->m_layer_middle_contourns_quads->AddNormal( leftNormal, leftNormal, leftNormal, leftNormal );
layerTriangles->m_layer_middle_contourns_quads->AddQuad( SFVEC3F( rightStart.x, rightStart.y, layer_z_top ),
SFVEC3F( rightEnd.x, rightEnd.y, layer_z_top ),
SFVEC3F( rightEnd.x, rightEnd.y, layer_z_bot ),
SFVEC3F( rightStart.x, rightStart.y, layer_z_bot ) );
const SFVEC3F rightNormal = SFVEC3F( -rightDir.y, rightDir.x, 0.0f );
layerTriangles->m_layer_middle_contourns_quads->AddNormal( rightNormal, rightNormal, rightNormal, rightNormal );
// Compute the outlines of the segment, and creates a polygon
// add right rounded end:
std::vector< SFVEC2F > roundedEndPointsStart;
std::vector< SFVEC2F > roundedEndPointsEnd;
roundedEndPointsStart.clear();
roundedEndPointsStart.reserve( nr_sides_per_circle + 2 );
roundedEndPointsEnd.clear();
roundedEndPointsEnd.reserve( nr_sides_per_circle + 2 );
roundedEndPointsStart.push_back( SFVEC2F( leftStart.x, leftStart.y ) );
roundedEndPointsEnd.push_back( SFVEC2F( leftEnd.x, leftEnd.y ) );
int delta = 3600 / nr_sides_per_circle;
for( int ii = delta; ii < 1800; ii += delta )
{
const SFVEC2F rotatedDirL = glm::rotate( leftDir, (float) ii * 2.0f * 3.14f / 3600.0f );
const SFVEC2F rotatedDirR = glm::rotate( rightDir, (float)(1800 - ii) * 2.0f * 3.14f / 3600.0f );
roundedEndPointsStart.push_back( SFVEC2F( start.x - rotatedDirL.y * radius, start.y + rotatedDirL.x * radius ) );
roundedEndPointsEnd.push_back( SFVEC2F( end.x - rotatedDirR.y * radius, end.y + rotatedDirR.x * radius ) );
}
roundedEndPointsStart.push_back( SFVEC2F( rightEnd.x, rightEnd.y ) );
roundedEndPointsEnd.push_back( SFVEC2F( rightStart.x, rightStart.y ) );
if( roundedEndPointsStart.size() > 1 )
{
for( unsigned int i = 0; i < ( roundedEndPointsStart.size() - 1 ); ++i )
{
const SFVEC2F &v0 = roundedEndPointsStart[i + 0];
const SFVEC2F &v1 = roundedEndPointsStart[i + 1];
layerTriangles->m_layer_middle_contourns_quads->AddQuad( SFVEC3F( v0.x, v0.y, layer_z_bot ),
SFVEC3F( v1.x, v1.y, layer_z_bot ),
SFVEC3F( v1.x, v1.y, layer_z_top ),
SFVEC3F( v0.x, v0.y, layer_z_top ) );
const SFVEC2F n0 = glm::normalize( v0 - start );
const SFVEC2F n1 = glm::normalize( v1 - start );
const SFVEC3F n0z = SFVEC3F( n0.x, n0.y, 0.0f );
const SFVEC3F n1z = SFVEC3F( n1.x, n1.y, 0.0f );
layerTriangles->m_layer_middle_contourns_quads->AddNormal( n0z, n1z, n1z, n0z );
}
}
roundedEndPointsStart.clear();
if( roundedEndPointsEnd.size() > 1 )
{
for( unsigned int i = 0; i < ( roundedEndPointsEnd.size() - 1 ); ++i )
{
const SFVEC2F &v0 = roundedEndPointsEnd[i + 0];
const SFVEC2F &v1 = roundedEndPointsEnd[i + 1];
layerTriangles->m_layer_middle_contourns_quads->AddQuad( SFVEC3F( v0.x, v0.y, layer_z_top ),
SFVEC3F( v1.x, v1.y, layer_z_top ),
SFVEC3F( v1.x, v1.y, layer_z_bot ),
SFVEC3F( v0.x, v0.y, layer_z_bot ) );
const SFVEC2F n0 = glm::normalize( v0 - end );
const SFVEC2F n1 = glm::normalize( v1 - end );
const SFVEC3F n0z = SFVEC3F( n0.x, n0.y, 0.0f );
const SFVEC3F n1z = SFVEC3F( n1.x, n1.y, 0.0f );
layerTriangles->m_layer_middle_contourns_quads->AddNormal( n0z, n1z, n1z, n0z );
}
}
roundedEndPointsEnd.clear();
}
break;
default:
{
}
break;
}
#if 0
// not yet used / implemented (can be used in future to clip the objects in the board borders
COBJECT2D *object2d_C = CSGITEM_FULL;
std::vector<const COBJECT2D *> *object2d_B = CSGITEM_EMPTY;
if( m_settings.GetFlag( FL_RENDER_SHOW_HOLES_IN_ZONES ) )
{
object2d_B = new std::vector<const COBJECT2D *>();
// Check if there are any layerhole that intersects this object
// Eg: a segment is cutted by a via hole or THT hole.
// /////////////////////////////////////////////////////////////
const MAP_CONTAINER_2D &layerHolesMap = m_settings.GetMapLayersHoles();
if( layerHolesMap.find(layer_id) != layerHolesMap.end() )
{
MAP_CONTAINER_2D::const_iterator ii_hole = layerHolesMap.find(layer_id);
const CBVHCONTAINER2D *containerLayerHoles2d = static_cast<const CBVHCONTAINER2D *>(ii_hole->second);
CONST_LIST_OBJECT2D intersectionList;
containerLayerHoles2d->GetListObjectsIntersects( object2d_A->GetBBox(), intersectionList );
if( !intersectionList.empty() )
{
for( CONST_LIST_OBJECT2D::const_iterator holeOnLayer = intersectionList.begin();
holeOnLayer != intersectionList.end();
holeOnLayer++ )
{
const COBJECT2D *hole2d = static_cast<const COBJECT2D *>(*holeOnLayer);
//if( object2d_A->Intersects( hole2d->GetBBox() ) )
//if( object2d_A->GetBBox().Intersects( hole2d->GetBBox() ) )
object2d_B->push_back( hole2d );
}
}
}
// Check if there are any THT that intersects this object
// /////////////////////////////////////////////////////////////
if( !m_settings.GetThroughHole_Inflated().GetList().empty() )
{
CONST_LIST_OBJECT2D intersectionList;
m_settings.GetThroughHole_Inflated().GetListObjectsIntersects( object2d_A->GetBBox(), intersectionList );
if( !intersectionList.empty() )
{
for( CONST_LIST_OBJECT2D::const_iterator hole = intersectionList.begin();
hole != intersectionList.end();
hole++ )
{
const COBJECT2D *hole2d = static_cast<const COBJECT2D *>(*hole);
//if( object2d_A->Intersects( hole2d->GetBBox() ) )
//if( object2d_A->GetBBox().Intersects( hole2d->GetBBox() ) )
object2d_B->push_back( hole2d );
}
}
}
if( object2d_B->empty() )
{
delete object2d_B;
object2d_B = CSGITEM_EMPTY;
}
}
if( (object2d_B == CSGITEM_EMPTY) &&
(object2d_C == CSGITEM_FULL) )
{
#if 0
create_3d_object_from( m_object_container, object2d_A, m_settings.GetLayerBottomZpos3DU( layer_id ),
m_settings.GetLayerTopZpos3DU( layer_id ),
materialLayer,
layerColor );
#else
CLAYERITEM *objPtr = new CLAYERITEM( object2d_A, m_settings.GetLayerBottomZpos3DU( layer_id ),
m_settings.GetLayerTopZpos3DU( layer_id ) );
objPtr->SetMaterial( materialLayer );
objPtr->SetColor( layerColor );
m_object_container.Add( objPtr );
#endif
}
else
{
#if 1
CITEMLAYERCSG2D *itemCSG2d = new CITEMLAYERCSG2D( object2d_A, object2d_B, object2d_C,
object2d_A->GetBoardItem() );
m_containerWithObjectsToDelete.Add( itemCSG2d );
CLAYERITEM *objPtr = new CLAYERITEM( itemCSG2d, m_settings.GetLayerBottomZpos3DU( layer_id ),
m_settings.GetLayerTopZpos3DU( layer_id ) );
objPtr->SetMaterial( materialLayer );
objPtr->SetColor( layerColor );
m_object_container.Add( objPtr );
#endif
}
#endif
}
// Create display list
// /////////////////////////////////////////////////////////////////////
m_ogl_disp_lists_layers[layer_id] = new CLAYERS_OGL_DISP_LISTS( *layerTriangles,
m_ogl_circle_texture,
layerColor );
}// for each layer on map
}
// Returns the specific channel in the range [-100:100]
float getRXChannel(rxChannel_e channel) {
return mapf(rxChannel[channel], RX_MIN_INPUT, RX_MAX_INPUT, -100.0f, 100.0f);
}
void control( void)
{
// hi rates
float ratemulti;
float ratemultiyaw;
if ( aux[RATES] )
{
ratemulti = HIRATEMULTI;
ratemultiyaw = HIRATEMULTIYAW;
}
else
{
ratemulti = 1.0;
ratemultiyaw = 1.0;
}
yawangle = yawangle + gyro[YAW]*looptime;
if ( auxchange[HEADLESSMODE] )
{
yawangle = 0;
}
if ( aux[HEADLESSMODE] )
{
float temp = rx[ROLL];
rx[ROLL] = rx[ROLL] * cosf( yawangle) - rx[PITCH] * sinf(yawangle );
rx[PITCH] = rx[PITCH] * cosf( yawangle) + temp * sinf(yawangle ) ;
}
error[ROLL] = rx[ROLL] * (float) MAX_RATE * DEGTORAD * ratemulti - gyro[ROLL];
error[PITCH] = rx[PITCH] * (float) MAX_RATE * DEGTORAD * ratemulti - gyro[PITCH];
error[YAW] = rx[YAW] * (float) MAX_RATEYAW * DEGTORAD * ratemultiyaw - gyro[YAW];
pid_precalc();
pid(ROLL);
pid(PITCH);
pid(YAW);
// map throttle so under 10% it is zero
float throttle = mapf(rx[3], 0 , 1 , -0.1 , 1 );
if ( throttle < 0 ) throttle = 0;
// turn motors off if throttle is off and pitch / roll sticks are centered
if ( failsafe || (throttle < 0.001f && (!ENABLESTIX|| (fabs(rx[ROLL]) < 0.5f && fabs(rx[PITCH]) < 0.5f ) ) ) )
{ // motors off
for ( int i = 0 ; i <= 3 ; i++)
{
pwm_set( i , 0 );
}
onground = 1;
thrsum = 0;
#ifdef MOTOR_FILTER
// reset the motor filter
for ( int i = 0 ; i <= 3 ; i++)
{
motorfilter( 0 , i);
}
#endif
}
else
{
onground = 0;
float mix[4];
// pidoutput[2] += motorchange;
#ifdef INVERT_YAW_PID
pidoutput[2] = -pidoutput[2];
#endif
mix[MOTOR_FR] = throttle - pidoutput[ROLL] - pidoutput[PITCH] + pidoutput[YAW]; // FR
mix[MOTOR_FL] = throttle + pidoutput[ROLL] - pidoutput[PITCH] - pidoutput[YAW]; // FL
mix[MOTOR_BR] = throttle - pidoutput[ROLL] + pidoutput[PITCH] - pidoutput[YAW]; // BR
mix[MOTOR_BL] = throttle + pidoutput[ROLL] + pidoutput[PITCH] + pidoutput[YAW]; // BL
#ifdef INVERT_YAW_PID
// we invert again cause it's used by the pid internally (for limit)
pidoutput[2] = -pidoutput[2];
#endif
#ifdef MOTOR_FILTER
for ( int i = 0 ; i <= 3 ; i++)
{
mix[i] = motorfilter( mix[i] , i);
}
#endif
for ( int i = 0 ; i <= 3 ; i++)
{
#ifndef NOMOTORS
pwm_set( i ,motormap( mix[i] ) );
#else
tempx[i] = motormap( mix[i] );
#endif
}
thrsum = 0;
for ( int i = 0 ; i <= 3 ; i++)
{
if ( mix[i] < 0 ) mix[i] = 0;
if ( mix[i] > 1 ) mix[i] = 1;
thrsum+= mix[i];
}
thrsum = thrsum / 4;
}// end motors on
}
/*
* Mail a message on standard input to the people indicated
* in the passed header. (Internal interface).
*/
void
mail1(struct header *hp, int use_to, char *orig_to)
{
pid_t p, pid;
int i, s, gotcha;
char **namelist, *deliver;
struct name *to, *np;
FILE *mtf, *fp;
int remote = rflag != NOSTR || rmail;
char **t;
char *deadletter;
char recfile[PATHSIZE];
/*
* Collect user's mail from standard input.
* Get the result as mtf.
*/
pid = (pid_t)-1;
if ((mtf = collect(hp)) == NULL)
return;
hp->h_seq = 1;
if (hp->h_subject == NOSTR)
hp->h_subject = sflag;
if (fsize(mtf) == 0 && hp->h_subject == NOSTR) {
printf(gettext("No message !?!\n"));
goto out;
}
if (intty) {
printf(gettext("EOT\n"));
flush();
}
/*
* If we need to use the To: line to determine the record
* file, save a copy of it before it's sorted below.
*/
if (use_to && orig_to == NOSTR && hp->h_to != NOSTR)
orig_to = strcpy((char *)salloc(strlen(hp->h_to)+1), hp->h_to);
else if (orig_to == NOSTR)
orig_to = "";
/*
* Now, take the user names from the combined
* to and cc lists and do all the alias
* processing.
*/
senderr = 0;
to = cat(extract(hp->h_bcc, GBCC),
cat(extract(hp->h_to, GTO),
extract(hp->h_cc, GCC)));
to = translate(outpre(elide(usermap(to))));
if (!senderr)
mapf(to, myname);
mechk(to);
for (gotcha = 0, np = to; np != NIL; np = np->n_flink)
if ((np->n_type & GDEL) == 0)
gotcha++;
hp->h_to = detract(to, GTO);
hp->h_cc = detract(to, GCC);
hp->h_bcc = detract(to, GBCC);
if ((mtf = infix(hp, mtf)) == NULL) {
fprintf(stderr, gettext(". . . message lost, sorry.\n"));
return;
}
rewind(mtf);
if (askme && isatty(0)) {
char ans[64];
puthead(hp, stdout, GTO|GCC|GBCC, 0);
printf(gettext("Send? "));
printf("[yes] ");
if (fgets(ans, sizeof(ans), stdin) && ans[0] &&
(tolower(ans[0]) != 'y' && ans[0] != '\n'))
goto dead;
}
if (senderr)
goto dead;
/*
* Look through the recipient list for names with /'s
* in them which we write to as files directly.
*/
i = outof(to, mtf);
rewind(mtf);
if (!gotcha && !i) {
printf(gettext("No recipients specified\n"));
goto dead;
}
if (senderr)
goto dead;
getrecf(orig_to, recfile, use_to, sizeof (recfile));
if (recfile != NOSTR && *recfile)
savemail(safeexpand(recfile), hp, mtf);
if (!gotcha)
goto out;
namelist = unpack(to);
if (debug) {
fprintf(stderr, "Recipients of message:\n");
for (t = namelist; *t != NOSTR; t++)
fprintf(stderr, " \"%s\"", *t);
fprintf(stderr, "\n");
return;
}
/*
* Wait, to absorb a potential zombie, then
* fork, set up the temporary mail file as standard
* input for "mail" and exec with the user list we generated
* far above. Return the process id to caller in case he
* wants to await the completion of mail.
*/
#ifdef VMUNIX
while (wait3((int *)0, WNOHANG, (struct rusage *)0) > 0)
;
#else
#ifdef preSVr4
wait((int *)0);
#else
while (waitpid((pid_t)-1, (int *)0, WNOHANG) > 0)
;
#endif
#endif
rewind(mtf);
pid = fork();
if (pid == (pid_t)-1) {
perror("fork");
dead:
deadletter = Getf("DEAD");
if (fp = fopen(deadletter,
value("appenddeadletter") == NOSTR ? "w" : "a")) {
chmod(deadletter, DEADPERM);
puthead(hp, fp, GMASK|GCLEN, fsize(mtf) - textpos);
fseek(mtf, textpos, 0);
lcwrite(deadletter, mtf, fp,
value("appenddeadletter") != NOSTR);
fclose(fp);
} else
perror(deadletter);
goto out;
}
if (pid == 0) {
sigchild();
#ifdef SIGTSTP
if (remote == 0) {
sigset(SIGTSTP, SIG_IGN);
sigset(SIGTTIN, SIG_IGN);
sigset(SIGTTOU, SIG_IGN);
}
#endif
sigset(SIGHUP, SIG_IGN);
sigset(SIGINT, SIG_IGN);
sigset(SIGQUIT, SIG_IGN);
s = fileno(mtf);
(void) fdwalk(closefd_walk, &s);
close(0);
dup(s);
close(s);
#ifdef CC
submit(getpid());
#endif /* CC */
if ((deliver = value("sendmail")) == NOSTR)
#ifdef SENDMAIL
deliver = SENDMAIL;
#else
deliver = MAIL;
#endif
execvp(safeexpand(deliver), namelist);
perror(deliver);
exit(1);
}
if (value("sendwait")!=NOSTR)
remote++;
out:
if (remote) {
while ((p = wait(&s)) != pid && p != (pid_t)-1)
;
if (s != 0)
senderr++;
pid = 0;
}
fclose(mtf);
return;
}
void control(void)
{
// hi rates
float ratemulti;
float ratemultiyaw;
float maxangle;
float anglerate;
#ifdef TOGGLE_IN
if ( auxchange[TOGGLE_IN] && !aux[TOGGLE_IN] )
{
ledcommand = 1;
aux[TOGGLE_OUT]=!aux[TOGGLE_OUT];
}
#endif
if (aux[RATES])
{
ratemulti = HIRATEMULTI;
ratemultiyaw = HIRATEMULTIYAW;
maxangle = MAX_ANGLE_HI;
anglerate = LEVEL_MAX_RATE_HI;
}
else
{
ratemulti = 1.0f;
ratemultiyaw = 1.0f;
maxangle = MAX_ANGLE_LO;
anglerate = LEVEL_MAX_RATE_LO;
}
for (int i = 0; i < 3; i++)
{
#ifdef STOCK_TX_AUTOCENTER
rxcopy[i] = rx[i] - autocenter[i];
#else
rxcopy[i] = rx[i];
#endif
}
flip_sequencer();
if ( (!aux[STARTFLIP])&&auxchange[STARTFLIP] )
{
start_flip();
auxchange[STARTFLIP]= 0;
}
if (auxchange[HEADLESSMODE])
{
yawangle = 0;
}
if ((aux[HEADLESSMODE]) )
{
yawangle = yawangle + gyro[2] * looptime;
while (yawangle < -3.14159265f)
yawangle += 6.28318531f;
while (yawangle > 3.14159265f)
yawangle -= 6.28318531f;
float temp = rxcopy[0];
rxcopy[0] = rxcopy[0] * fastcos( yawangle) - rxcopy[1] * fastsin(yawangle );
rxcopy[1] = rxcopy[1] * fastcos( yawangle) + temp * fastsin(yawangle ) ;
}
else
{
yawangle = 0;
}
// check for acc calibration
int command = gestures2();
if (command)
{
if (command == 3)
{
gyro_cal(); // for flashing lights
acc_cal();
savecal();
// reset loop time
extern unsigned lastlooptime;
lastlooptime = gettime();
}
else
{
ledcommand = 1;
if (command == 2)
{
aux[CH_AUX1] = 1;
}
if (command == 1)
{
aux[CH_AUX1] = 0;
}
}
}
if ( controls_override)
{
for ( int i = 0 ; i < 3 ; i++)
{
rxcopy[i] = rx_override[i];
}
ratemulti = 1.0f;
maxangle = MAX_ANGLE_HI;
anglerate = LEVEL_MAX_RATE_HI;
}
pid_precalc();
if ((aux[LEVELMODE]||level_override)&&!acro_override)
{ // level mode
extern void stick_vector( float );
extern float errorvect[];
stick_vector( maxangle);
float yawerror[3];
extern float GEstG[3];
float yawrate = rxcopy[2] * (float) MAX_RATEYAW * DEGTORAD * ratemultiyaw* ( 1/ 2048.0f);
yawerror[0] = GEstG[1] * yawrate;
yawerror[1] = - GEstG[0] * yawrate;
yawerror[2] = GEstG[2] * yawrate;
angleerror[0] = errorvect[0] * RADTODEG *1.1f;
angleerror[1] = errorvect[1] * RADTODEG *1.1f;
for ( int i = 0 ; i <2; i++)
{
error[i] = apid(i) * anglerate * DEGTORAD + yawerror[i] - gyro[i];
}
error[2] = yawerror[2] - gyro[2];
}
else
{ // rate mode
error[0] = rxcopy[0] * MAX_RATE * DEGTORAD * ratemulti - gyro[0];
error[1] = rxcopy[1] * MAX_RATE * DEGTORAD * ratemulti - gyro[1];
error[2] = rxcopy[2] * MAX_RATEYAW * DEGTORAD * ratemultiyaw - gyro[2];
// reduce angle Iterm towards zero
extern float aierror[3];
aierror[0] = 0.0f;
aierror[1] = 0.0f;
}
pid(0);
pid(1);
pid(2);
// map throttle so under 10% it is zero
float throttle = mapf(rx[3], 0, 1, -0.1, 1);
if (throttle < 0)
throttle = 0;
if (throttle > 1.0f)
throttle = 1.0f;
#ifdef AIRMODE_HOLD_SWITCH
if (failsafe || aux[AIRMODE_HOLD_SWITCH] || throttle < 0.001f && !onground_long)
{
onground_long = 0;
#else
// turn motors off if throttle is off and pitch / roll sticks are centered
if (failsafe || (throttle < 0.001f && ( !ENABLESTIX || !onground_long || aux[LEVELMODE] || level_override || (fabsf(rx[0]) < (float) ENABLESTIX_TRESHOLD && fabsf(rx[1]) < (float) ENABLESTIX_TRESHOLD))))
{ // motors off
#endif
onground = 1;
if ( onground_long )
{
if ( gettime() - onground_long > 1000000)
{
onground_long = 0;
}
}
#ifdef MOTOR_BEEPS
extern void motorbeep( void);
motorbeep();
#endif
thrsum = 0;
for (int i = 0; i <= 3; i++)
{
pwm_set(i, 0);
}
// reset the overthrottle filter
lpf(&overthrottlefilt, 0.0f, 0.72f); // 50hz 1khz sample rate
lpf(&underthrottlefilt, 0.0f, 0.72f); // 50hz 1khz sample rate
#ifdef MOTOR_FILTER
// reset the motor filter
for (int i = 0; i <= 3; i++)
{
motorfilter(0, i);
}
#endif
#ifdef THROTTLE_TRANSIENT_COMPENSATION
// reset hpf filter;
throttlehpf(0);
#endif
#ifdef STOCK_TX_AUTOCENTER
for( int i = 0 ; i <3;i++)
{
if ( rx[i] == lastrx[i] )
{
consecutive[i]++;
}
else consecutive[i] = 0;
lastrx[i] = rx[i];
if ( consecutive[i] > 1000 && fabsf( rx[i]) < 0.1f )
{
autocenter[i] = rx[i];
}
}
#endif
// end motors off / failsafe / onground
}
else
{
// motors on - normal flight
onground_long = gettime();
#ifdef THROTTLE_TRANSIENT_COMPENSATION
throttle += 7.0f * throttlehpf(throttle);
if (throttle < 0)
throttle = 0;
if (throttle > 1.0f)
throttle = 1.0f;
#endif
// throttle angle compensation
#ifdef AUTO_THROTTLE
if (aux[LEVELMODE])
{
float autothrottle = fastcos(attitude[0] * DEGTORAD) * fastcos(attitude[1] * DEGTORAD);
float old_throttle = throttle;
if (autothrottle <= 0.5f)
autothrottle = 0.5f;
throttle = throttle / autothrottle;
// limit to 90%
if (old_throttle < 0.9f)
if (throttle > 0.9f)
throttle = 0.9f;
if (throttle > 1.0f)
throttle = 1.0f;
}
#endif
#ifdef LVC_PREVENT_RESET
extern float vbatt;
if (vbatt < (float) LVC_PREVENT_RESET_VOLTAGE) throttle = 0;
#endif
onground = 0;
float mix[4];
if ( controls_override)
{// change throttle in flip mode
throttle = rx_override[3];
}
#ifdef INVERT_YAW_PID
pidoutput[2] = -pidoutput[2];
#endif
mix[MOTOR_FR] = throttle - pidoutput[0] - pidoutput[1] + pidoutput[2]; // FR
mix[MOTOR_FL] = throttle + pidoutput[0] - pidoutput[1] - pidoutput[2]; // FL
mix[MOTOR_BR] = throttle - pidoutput[0] + pidoutput[1] - pidoutput[2]; // BR
mix[MOTOR_BL] = throttle + pidoutput[0] + pidoutput[1] + pidoutput[2]; // BL
#ifdef INVERT_YAW_PID
// we invert again cause it's used by the pid internally (for limit)
pidoutput[2] = -pidoutput[2];
#endif
#if ( defined MIX_LOWER_THROTTLE || defined MIX_INCREASE_THROTTLE)
//#define MIX_INCREASE_THROTTLE
// options for mix throttle lowering if enabled
// 0 - 100 range ( 100 = full reduction / 0 = no reduction )
#ifndef MIX_THROTTLE_REDUCTION_PERCENT
#define MIX_THROTTLE_REDUCTION_PERCENT 100
#endif
// lpf (exponential) shape if on, othewise linear
//#define MIX_THROTTLE_FILTER_LPF
// limit reduction and increase to this amount ( 0.0 - 1.0)
// 0.0 = no action
// 0.5 = reduce up to 1/2 throttle
//1.0 = reduce all the way to zero
#ifndef MIX_THROTTLE_REDUCTION_MAX
#define MIX_THROTTLE_REDUCTION_MAX 0.5
#endif
#ifndef MIX_THROTTLE_INCREASE_MAX
#define MIX_THROTTLE_INCREASE_MAX 0.2
#endif
#ifndef MIX_MOTOR_MAX
#define MIX_MOTOR_MAX 1.0f
#endif
float overthrottle = 0;
float underthrottle = 0.001f;
for (int i = 0; i < 4; i++)
{
if (mix[i] > overthrottle)
overthrottle = mix[i];
if (mix[i] < underthrottle)
underthrottle = mix[i];
}
overthrottle -= MIX_MOTOR_MAX ;
if (overthrottle > (float)MIX_THROTTLE_REDUCTION_MAX)
overthrottle = (float)MIX_THROTTLE_REDUCTION_MAX;
#ifdef MIX_THROTTLE_FILTER_LPF
if (overthrottle > overthrottlefilt)
lpf(&overthrottlefilt, overthrottle, 0.82); // 20hz 1khz sample rate
else
lpf(&overthrottlefilt, overthrottle, 0.72); // 50hz 1khz sample rate
#else
if (overthrottle > overthrottlefilt)
overthrottlefilt += 0.005f;
else
overthrottlefilt -= 0.01f;
#endif
// over
if (overthrottlefilt > (float)MIX_THROTTLE_REDUCTION_MAX)
overthrottlefilt = (float)MIX_THROTTLE_REDUCTION_MAX;
if (overthrottlefilt < -0.1f)
overthrottlefilt = -0.1;
overthrottle = overthrottlefilt;
if (overthrottle < 0.0f)
overthrottle = -0.0001f;
// reduce by a percentage only, so we get an inbetween performance
overthrottle *= ((float)MIX_THROTTLE_REDUCTION_PERCENT / 100.0f);
#ifndef MIX_LOWER_THROTTLE
// disable if not enabled
overthrottle = -0.0001f;
#endif
#ifdef MIX_INCREASE_THROTTLE
// under
if (underthrottle < -(float)MIX_THROTTLE_INCREASE_MAX)
underthrottle = -(float)MIX_THROTTLE_INCREASE_MAX;
#ifdef MIX_THROTTLE_FILTER_LPF
if (underthrottle < underthrottlefilt)
lpf(&underthrottlefilt, underthrottle, 0.82); // 20hz 1khz sample rate
else
lpf(&underthrottlefilt, underthrottle, 0.72); // 50hz 1khz sample rate
#else
if (underthrottle < underthrottlefilt)
underthrottlefilt -= 0.005f;
else
underthrottlefilt += 0.01f;
#endif
// under
if (underthrottlefilt < - (float)MIX_THROTTLE_REDUCTION_MAX)
underthrottlefilt = - (float)MIX_THROTTLE_REDUCTION_MAX;
if (underthrottlefilt > 0.1f)
underthrottlefilt = 0.1;
underthrottle = underthrottlefilt;
if (underthrottle > 0.0f)
underthrottle = 0.0001f;
underthrottle *= ((float)MIX_THROTTLE_REDUCTION_PERCENT / 100.0f);
#endif
if (overthrottle > 0 || underthrottle < 0 )
{ // exceeding max motor thrust
float temp = overthrottle + underthrottle;
for (int i = 0; i < 4; i++)
{
mix[i] -= temp;
}
}
// end MIX_LOWER_THROTTLE
#endif
#ifdef MOTOR_FILTER
for (int i = 0; i < 4; i++)
{
mix[i] = motorfilter(mix[i], i);
}
#endif
#ifdef CLIP_FF
float clip_ff(float motorin, int number);
for (int i = 0; i < 4; i++)
{
mix[i] = clip_ff(mix[i], i);
}
#endif
for (int i = 0; i < 4; i++)
{
float test = motormap(mix[i]);
#ifdef MOTORS_TO_THROTTLE
test = throttle;
// flash leds in valid throttle range
ledcommand = 1;
// for battery estimation
mix[i] = throttle;
#warning "MOTORS TEST MODE"
#endif
#ifdef MOTOR_MIN_ENABLE
if (test < (float) MOTOR_MIN_VALUE)
{
test = (float) MOTOR_MIN_VALUE;
}
#endif
#ifdef MOTOR_MAX_ENABLE
if (test > (float) MOTOR_MAX_VALUE)
{
test = (float) MOTOR_MAX_VALUE;
}
#endif
#ifndef NOMOTORS
//normal mode
pwm_set( i , test );
#else
#warning "NO MOTORS"
#endif
}
thrsum = 0;
for (int i = 0; i < 4; i++)
{
if (mix[i] < 0)
mix[i] = 0;
if (mix[i] > 1)
mix[i] = 1;
thrsum += mix[i];
}
thrsum = thrsum / 4;
} // end motors on
imu_calc();
}
/////////////////////////////
/////////////////////////////
#ifdef MOTOR_CURVE_6MM_490HZ
// the old map for 490Hz
float motormap(float input)
{
// this is a thrust to pwm function
// float 0 to 1 input and output
// output can go negative slightly
// measured eachine motors and prop, stock battery
// a*x^2 + b*x + c
// a = 0.262 , b = 0.771 , c = -0.0258
if (input > 1)
input = 1;
if (input < 0)
input = 0;
input = input * input * 0.262f + input * (0.771f);
input += -0.0258f;
return input;
}
#endif
// 8k pwm is where the motor thrust is relatively linear for the H8 6mm motors
// it's due to the motor inductance cancelling the nonlinearities.
#ifdef MOTOR_CURVE_NONE
float motormap(float input)
{
return input;
}
#endif
#ifdef MOTOR_CURVE_85MM_8KHZ
// Hubsan 8.5mm 8khz pwm motor map
// new curve
float motormap(float input)
{
// Hubsan 8.5mm motors and props
if (input > 1)
input = 1;
if (input < 0)
input = 0;
input = input * input * 0.683f + input * (0.262f);
input += 0.06f;
return input;
}
#endif
#ifdef MOTOR_CURVE_85MM_8KHZ_OLD
// Hubsan 8.5mm 8khz pwm motor map
float motormap(float input)
{
// Hubsan 8.5mm motors and props
if (input > 1)
input = 1;
if (input < 0)
input = 0;
input = input * input * 0.789f + input * (0.172f);
input += 0.04f;
return input;
}
#endif
#ifdef MOTOR_CURVE_85MM_32KHZ
// Hubsan 8.5mm 8khz pwm motor map
float motormap(float input)
{
// Hubsan 8.5mm motors and props
if (input > 1)
input = 1;
if (input < 0)
input = 0;
input = input * input * 0.197f + input * (0.74f);
input += 0.067f;
return input;
}
#endif
float hann_lastsample[4];
float hann_lastsample2[4];
// hanning 3 sample filter
float motorfilter(float motorin, int number)
{
float ans = motorin * 0.25f + hann_lastsample[number] * 0.5f + hann_lastsample2[number] * 0.25f;
hann_lastsample2[number] = hann_lastsample[number];
hann_lastsample[number] = motorin;
return ans;
}
void Controller::Update() {
//full throttle cut
if (commands_.throttle <= kZeroThrottleThreshold || commands_.throttle > 1.25) {
//power down all motors
escFR.writeMicroseconds(kMinPulseWidth);
escFL.writeMicroseconds(kMinPulseWidth);
escBR.writeMicroseconds(kMinPulseWidth);
escBL.writeMicroseconds(kMinPulseWidth);
yaw_error_sum_ = 0;
attitude_error_sum_ = 0;
bank_error_sum_ = 0;
last_frame_ = micros();
return;
}
this_frame_ = micros();
dt_ = this_frame_ - last_frame_;
last_frame_ = this_frame_;
dt_ /= 1000000;
Orientation current_orientation_;
imu_->GetOrientation(current_orientation_);
// update current error
Orientation current_error;
float current_yaw_error = commands_.yaw - (current_orientation_.heading - last_heading_) / dt_ * 1000000;
current_error.attitude = commands_.attitude + current_orientation_.attitude;
current_error.bank = commands_.bank - current_orientation_.bank;
yaw_error_sum_ += current_yaw_error * dt_;
attitude_error_sum_ += current_error.attitude * dt_;
bank_error_sum_ += current_error.bank * dt_;
yaw_error_sum_ = constrain(yaw_error_sum_, -kMaxYawITerm, kMaxYawITerm);
attitude_error_sum_ = constrain(attitude_error_sum_, -kMaxAttitudeITerm, kMaxAttitudeITerm);
bank_error_sum_ = constrain(bank_error_sum_, -kMaxBankITerm, kMaxBankITerm);
// calculate error derivative
float old_aed = attitude_error_diff;
float old_bed = bank_error_diff;
attitude_error_diff = ((current_error.attitude - attitude_error_last_)
+ (commands_.attitude - last_commands_.attitude)) / dt_;
aed_history_sum -= aed_history[history_index];
aed_history[history_index] = attitude_error_diff;
aed_history_sum += attitude_error_diff;
attitude_error_diff = aed_history_sum / hist_amt;
bank_error_diff = ((current_error.bank - bank_error_last_)
+ (commands_.bank - last_commands_.bank)) / dt_;
bed_history_sum -= bed_history[history_index];
bed_history[history_index] = bank_error_diff;
bed_history_sum += bank_error_diff;
bank_error_diff = bed_history_sum / hist_amt;
history_index++;
history_index %= hist_amt;
attitude_error_last_ = current_error.attitude;
bank_error_last_ = current_error.bank;
float thr;
if (commands_.hold_altitude) {
if (altitude_setpoint_ == NAN) {
imu_->GetAltitude(altitude_setpoint_);
thr = last_throttle_;
} else {
float curr_altitude;
imu_->GetAltitude(curr_altitude);
thr = last_throttle_ + kP_altitude * (altitude_setpoint_ - curr_altitude);
}
} else {
altitude_setpoint_ = NAN;
thr = commands_.throttle * kThrottleScaling;
}
last_throttle_ = thr;
//determind quad adjustments (in % throttle)
float y_adj = kP_yaw * current_yaw_error
+ kI_yaw * yaw_error_sum_;
y_adj=0; //remove this later
float a_adj = commands_.kp_adj * kP_attitude * current_error.attitude
+ kI_attitude * attitude_error_sum_
+ kD_attitude * commands_.ki_adj * attitude_error_diff;
float b_adj = commands_.kp_adj * kP_bank * current_error.bank
+ kI_bank * bank_error_sum_
+ kD_bank * commands_.ki_adj * bank_error_diff;
#ifdef DEBUG_PID
{
static int i = 0;
i++;
if (i == 50) {
printed = true;
Serial.print(F("thr: "));
Serial.print(thr);
Serial.print("\t");
Serial.print(F("y_ad: "));
Serial.print(y_adj);
Serial.print("\t");
Serial.print(F("a_ad: "));
Serial.print(a_adj);
Serial.print("\t");
Serial.print(F("b_ad: "));
Serial.print(b_adj);
Serial.print("\t");
i = 0;
}
}
#endif
//determine ESC pulse widths
float escFRVal = mapf(thr - y_adj - a_adj - b_adj, 0, 1, kMinPulseWidth, kMaxPulseWidth);
float escFLVal = mapf(thr + y_adj - a_adj + b_adj, 0, 1, kMinPulseWidth, kMaxPulseWidth);
float escBRVal = mapf(thr + y_adj + a_adj - b_adj, 0, 1, kMinPulseWidth, kMaxPulseWidth);
float escBLVal = mapf(thr - y_adj + a_adj + b_adj, 0, 1, kMinPulseWidth, kMaxPulseWidth);
#ifdef DEBUG_OUTPUTS
{
static int i = 0;
i++;
if (i == 20) {
printed = true;
Serial.print(F("FR: "));
Serial.print(escFRVal);
Serial.print(F("\tFL: "));
Serial.print(escFLVal);
Serial.print(F("\tBR: "));
Serial.print(escBRVal);
Serial.print(F("\tBL: "));
Serial.print(escBLVal);
Serial.print("\t");
i = 0;
}
}
#endif
escFR.writeMicroseconds(escFRVal);
escFL.writeMicroseconds(escFLVal);
escBR.writeMicroseconds(escBRVal);
escBL.writeMicroseconds(escBLVal);
}
float ADXL377::getZ()
{
return mapf(analogRead(zPin_));
}
float read20Volts(int pin){
int voltage = analogRead(pin);
//Serial.print(voltage);
return mapf(voltage,0,1023,0,20);
}
float readBrdCurrent(int pin){
int voltage = analogRead(pin);
//Serial.print(voltage);
return mapf(voltage,0,1023,0,2);
}
double Life::mapState(double state) {
return (float)((int)mapf(state, 0.0, numStates() - 1));
}
void pwm_set( uint8_t number , float pwm)
{
if ( pwm < 0 ) pwm = 0;
pwm = mapf ( pwm , 0 , 1 , ( (float) PWMTOP/PWMTOP_US)*ESC_MIN , ( (float) PWMTOP/PWMTOP_US)*ESC_MAX );
if ( onground ) pwm = ((float)PWMTOP/PWMTOP_US) * ESC_THROTTLEOFF;
if ( failsafe )
{
if ( !pwm_failsafe_time )
{
pwm_failsafe_time = gettime();
}
else
{
// 100mS after failsafe we turn off the signal (for safety while flashing)
if ( gettime() - pwm_failsafe_time > 100000 )
{
pwm = ((float)PWMTOP/PWMTOP_US) * ESC_FAILSAFE;
}
}
}
else
{
pwm_failsafe_time = 0;
}
if ( pwm > ((float)PWMTOP/PWMTOP_US)*ESC_MAX ) pwm = ((float)PWMTOP/PWMTOP_US)*ESC_MAX ;
#ifdef ONESHOT_125_ENABLE
pwm = pwm/8;
#endif
pwm = lroundf(pwm);
if ( pwm < 0 ) pwm = 0;
if ( pwm > PWMTOP ) pwm = PWMTOP;
switch( number)
{
case 0:
TIMER1->CHCC1 = (uint32_t) pwm;
break;
case 1:
TIMER3->CHCC4 = (uint32_t) pwm;
break;
case 2:
TIMER1->CHCC2 = (uint32_t) pwm;
break;
case 3:
TIMER1->CHCC3 = (uint32_t) pwm;
break;
default:
// handle error;
//
break;
}
}
static int route_map_ifs(struct ifstat_driver *driver,
int (*mapf)(char *name, struct if_msghdr *ifmsg, void *data),
void *mdata) {
struct route_driver_data *data = driver->data;
int iflist[] = {
CTL_NET, PF_ROUTE, 0, AF_LINK, NET_RT_IFLIST, 0
};
struct if_msghdr *ifm;
struct sockaddr_dl *dl;
char *ptr;
char ifname[IFNAMSIZ + 1];
size_t len;
if (data->size != 0) { /* try with current buf size */
len = data->size;
if (sysctl(iflist, sizeof(iflist) / sizeof(int),
data->buf, &len, NULL, 0) < 0) {
if (errno != ENOMEM) {
ifstat_perror("sysctl");
return 0;
}
/* buffer too small */
free (data->buf);
data->size = 0;
}
}
if (data->size == 0) {
/* ask for size */
if (sysctl(iflist, sizeof(iflist) / sizeof(int),
NULL, &len, NULL, 0) < 0) {
ifstat_perror("sysctl");
return 0;
}
if ((data->buf = malloc(len)) < 0) {
ifstat_perror("malloc");
return 0;
}
if (sysctl(iflist, sizeof(iflist) / sizeof(int),
data->buf, &len, NULL, 0) < 0) {
ifstat_perror("sysctl");
return 0;
}
}
/* browse interfaes */
for (ptr = data->buf; ptr < data->buf + len; ptr += ifm->ifm_msglen) {
ifm = (struct if_msghdr *) ptr;
if (ifm->ifm_type != RTM_IFINFO)
continue;
if (ifm->ifm_msglen <= sizeof(struct if_msghdr)) /* no address */
continue;
dl = (struct sockaddr_dl *) (ptr + sizeof(struct if_msghdr));
if (dl->sdl_family != AF_LINK)
continue;
if (dl->sdl_nlen > (sizeof(ifname) - 1))
dl->sdl_nlen = sizeof(ifname) - 1;
memcpy(ifname, dl->sdl_data, dl->sdl_nlen);
ifname[dl->sdl_nlen] = '\0';
if (!mapf(ifname, ifm, mdata))
return 0;
}
return 1;
}