int Ctcs::Report_Status()
{
int changebw;
dt_rate_t dlrate, ulrate, dlimit, ulimit;
if( DT_TRACKER_READY != m_status ) return 0;
dlrate = Self.RateDL();
ulrate = Self.RateUL();
dlimit = *cfg_max_bandwidth_down;
ulimit = *cfg_max_bandwidth_up;
changebw = (
compset(m_ctstatus, dlrate) |
compset(m_ctstatus, ulrate) |
compset(m_ctstatus, dlimit) |
compset(m_ctstatus, ulimit) );
m_statustime = now;
if( !m_sent_ctstatus ||
(TRACKER.GetStatus() && now > m_sent_ctstatus_time+30) ){
return Send_Status();
}else return ( changebw || !m_sent_ctbw ) ? Send_bw() : 0;
}
void Commander::Data_Exchange(void)
{
static u8 cnt = 0;
switch(cnt)
{
case 1:
f.Send_RCData = 1;
break;
case 2:
f.Send_MotoPwm = 1;
break;
case 30:
cnt = 0;
break;
default:
if(cnt%3)
f.Send_Senser = 1;
else
f.Send_Status = 1;
}
cnt++;
if(f.Send_Status){
f.Send_Status = 0;
Send_Status();
}
if(f.Send_Senser){
f.Send_Senser = 0;
Send_Senser();
}
if(f.Send_RCData){
f.Send_RCData = 0;
Send_RCData();
}
if(f.Send_MotoPwm){
f.Send_MotoPwm = 0;
Send_MotoPWM();
}
if(f.Send_PID1){
f.Send_PID1 = 0;
Send_PID1();
}
if(f.Send_PID2){
f.Send_PID2 = 0;
Send_PID2();
}
}
void Data_Exchange(void)
{
static int cnt = 0;
switch(cnt)
{
case SEND_STATES:
Send_Status();
cnt++;
break;
case SEND_SENSER:
Send_Senser();
cnt++;
break;
case SEND_PID1:
Send_PID1();
cnt++;
break;
case SEND_PID2:
Send_PID2();
cnt++;
break;
case SEND_RC:
Send_RCData();
cnt++;
break;
case SEND_MOTORPWM:
Send_MotoPWM();
cnt++;
break;
case SEND_DEBUG:
Send_DEBUG();
cnt++;
break;
default:
cnt=0;
break;
}
}
int Ctcs::CheckMessage()
{
ssize_t r;
size_t q;
r = in_buffer.FeedIn(m_sock);
// This differs from tracker.cpp since we maintain a persistent connection.
if( r == 0 ) return 0; // no data (should return an error)
q = in_buffer.Count();
if( !q ){
int error = 0;
socklen_t n = sizeof(error);
if( getsockopt(m_sock, SOL_SOCKET, SO_ERROR, &error, &n) < 0 )
error = errno;
if( error != 0 ) CONSOLE.Warning(2,
"warn, received nothing from CTCS: %s", strerror(error));
Reset(5);
return -1;
}
const char *s, *p, *msgbuf;
while( in_buffer.Count() ){
msgbuf = in_buffer.BasePointer();
p = (const char *)memchr(msgbuf, '\r', in_buffer.Count());
s = (const char *)memchr(msgbuf, '\n', in_buffer.Count());
if( p && s > p ) s = p;
if( !s ) break;
if( *cfg_verbose && s != msgbuf )
CONSOLE.Debug("CTCS: %.*s", (int)(s - msgbuf), msgbuf);
if( !strncmp("SETDLIMIT", msgbuf, 9) ){
dt_rate_t arg = (dt_rate_t)strtod(msgbuf+10, NULL);
if( !BTCONTENT.IsFull() || arg < *cfg_max_bandwidth_down ){
cfg_max_bandwidth_down = arg;
}
}else if( !strncmp("SETULIMIT", msgbuf, 9) ){
cfg_max_bandwidth_up = (dt_rate_t)strtod(msgbuf+10, NULL);
}else if( !strncmp("SENDPEERS", msgbuf, 9) ){
Send_Peers();
}else if( !strncmp("SENDSTATUS", msgbuf, 10) ){
Send_Status();
}else if( !strncmp("SENDCONF", msgbuf, 8) ){
Send_Config();
}else if( !strncmp("CTCONFIG", msgbuf, 8) ){
Set_Config(msgbuf);
}else if( !strncmp("SENDDETAIL", msgbuf, 10) ){
Send_Detail();
}else if( !strncmp("CTQUIT", msgbuf, 6) ){
CONSOLE.Print("CTCS sent Quit command");
TRACKER.Stop();
}else if( !strncmp("CTRESTART", msgbuf, 9) ){
TRACKER.RestartTracker();
}else if( !strncmp("CTUPDATE", msgbuf, 8) ){
TRACKER.Update();
}else if( !strncmp("PROTOCOL", msgbuf, 8) ){
int proto = atoi(msgbuf+9);
if( proto <= CTCS_PROTOCOL ) m_protocol = proto;
else m_protocol = CTCS_PROTOCOL;
}else if( s != msgbuf ){
if(*cfg_verbose)
CONSOLE.Debug("unknown CTCS message: %.*s", (int)(s - msgbuf), msgbuf);
}
in_buffer.PickUp(s - msgbuf + 1);
}
m_last_timestamp = now;
return 0;
}
void ANO_DT::Data_Exchange(void)
{
static u8 cnt = 0;
static u8 senser_cnt = 10;
static u8 user_cnt = 10;
static u8 status_cnt = 15;
static u8 rcdata_cnt = 50;
static u8 motopwm_cnt = 50;
static u8 power_cnt = 50;
static u8 senser2_cnt = 50;
if((cnt % senser_cnt) == (senser_cnt-1))
f.send_senser = 1;
if((cnt % senser2_cnt) == (senser2_cnt-1))
f.send_senser2 = 1;
if((cnt % user_cnt) == (user_cnt-1))
f.send_user = 1;
if((cnt % status_cnt) == (status_cnt-1))
f.send_status = 1;
if((cnt % rcdata_cnt) == (rcdata_cnt-1))
f.send_rcdata = 1;
if((cnt % motopwm_cnt) == (motopwm_cnt-1))
f.send_motopwm = 1;
if((cnt % power_cnt) == (power_cnt-1))
f.send_power = 1;
cnt++;
/////////////////////////////////////////////////////////////////////////////////////
if(f.send_version)
{
f.send_version = 0;
Send_Version(1,300,110,400,0);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_status)
{
f.send_status = 0;
Send_Status(imu.angle.x,imu.angle.y,imu.angle.z,0,0,ano.f.ARMED);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_user)
{
f.send_user = 0;
//Send_User();
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_senser)
{
f.send_senser = 0;
Send_Senser(imu.Acc.x,imu.Acc.y,imu.Acc.z,
imu.Gyro.x,imu.Gyro.y,imu.Gyro.z,
0,0,0);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_senser2)
{
f.send_senser2 = 0;
Send_Senser2(0,0);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_rcdata)
{
f.send_rcdata = 0;
Send_RCData(rc.rawData[THROTTLE],rc.rawData[YAW],rc.rawData[ROLL],rc.rawData[PITCH],
rc.rawData[AUX1],rc.rawData[AUX2],rc.rawData[AUX3],
rc.rawData[AUX4],rc.rawData[AUX5],rc.rawData[AUX6]);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_motopwm)
{
f.send_motopwm = 0;
int16_t Moto_PWM[4];
motor.getPWM(Moto_PWM);
for(u8 i=0;i<4;i++)
Moto_PWM[i] -= 1000;
Send_MotoPWM(Moto_PWM[0],Moto_PWM[1],Moto_PWM[2],Moto_PWM[3],0,0,0,0);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_power)
{
f.send_power = 0;
Send_Power(123,456);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_pid1)
{
f.send_pid1 = 0;
Send_PID(1,fc.pid[PIDROLL].kP,fc.pid[PIDROLL].kI,fc.pid[PIDROLL].kD,
fc.pid[PIDPITCH].kP,fc.pid[PIDPITCH].kI,fc.pid[PIDPITCH].kD,
fc.pid[PIDYAW].kP,fc.pid[PIDYAW].kI,fc.pid[PIDYAW].kD);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_pid2)
{
f.send_pid2 = 0;
Send_PID(2,fc.pid[PIDANGLE].kP,fc.pid[PIDANGLE].kI,fc.pid[PIDANGLE].kD,
fc.pid[PIDMAG].kP,fc.pid[PIDMAG].kI,fc.pid[PIDMAG].kD,
fc.pid[PIDVELZ].kP,fc.pid[PIDVELZ].kI,fc.pid[PIDVELZ].kD);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_pid3)
{
f.send_pid3 = 0;
Send_PID(3,fc.pid[PIDALT].kP,fc.pid[PIDALT].kI,fc.pid[PIDALT].kD,
0,0,0,
0,0,0);
}
}
