File content as of revision 5:0a6df7e647f3:

#include "decawave_network.h"

DecaWaveNetwork::DecaWaveNetwork(DW1000& DW) : dw(DW)
{
    isAnchor = true;
    overflow = false;
    address = 0;
    rxTimestamp = 0;
    timediffRec = 0;
    timediffSend = 0;
    for (int i = 0; i < MAX_NODES; i++)
        acknowledgement[i] = true;

    dw.setCallbacks(this, &DecaWaveNetwork::callbackRX, &DecaWaveNetwork::callbackTX);

    LocalTimer.start();
    //checkConn.attach(this,&DecaWaveNetwork::checkConnectivity, 10.0);
    dw.startRX();
}

void DecaWaveNetwork::callbackRX()
{
    int n = dw.getFramelength();
    dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, n);



    if (receivedFrame.destination == address){
        switch (receivedFrame.type) {
            case PING:
                rxTimestamp = dw.getRXTimestamp();
                receiverTimestamps[receivedFrame.source][0] = rxTimestamp;      //Save the first timestamp on the receiving node/anchor (T_rp)
                sendDelayedAnswer(receivedFrame.source, ANCHOR_RESPONSE, rxTimestamp);
                break;
            case ANCHOR_RESPONSE:
                rxTimestamp = dw.getRXTimestamp();
                senderTimestamps[receivedFrame.source][1] = rxTimestamp;        //Save the second timestamp on the sending node/beacon (T_rr)
                sendDelayedAnswer(receivedFrame.source, 3, rxTimestamp);
                break;
            case BEACON_RESPONSE:
                rxTimestamp = dw.getRXTimestamp();
                receiverTimestamps[receivedFrame.source][2] = rxTimestamp;      //Save the third timestamp on the receiving node/anchor (T_rf)

                correctReceiverTimestamps(receivedFrame.source);                //Correct the timestamps for the case of a counter overflow
                //calculation of the summand on the receiving node/anchor
                timediffRec = - 2*receiverTimestamps[receivedFrame.source][1] + receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2];
                sendTransferFrame(receivedFrame.source, timediffRec );
                break;
            case TRANSFER_FRAME:
                //calculation of the summand on the sending node/beacon
                timediffSend = 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2];
                //calculation of the resulting sum of all four ToFs.
                tofs[receivedFrame.source] = receivedFrame.signedTime + timediffSend;
                acknowledgement[receivedFrame.source] = true;
                break;
            default :
                break;
        }
    } else{
        
    memcpy(&mavlink_buffer,&receivedFrame,n);
   // int i;

   /* for(int i=0; i<n; i++) {
        printf("%X",mavlink_buffer[i]);
    }
    printf("\r\n");*/
    } // end else


    parseMavlinkMsg(n);

    dw.startRX();
}

void DecaWaveNetwork::callbackTX()
{
    switch (rangingFrame.type) {
        case PING:
            senderTimestamps[rangingFrame.destination][0] = dw.getTXTimestamp();    //Save the first timestamp on the sending node/beacon (T_sp)
            break;
        case ANCHOR_RESPONSE:
            receiverTimestamps[rangingFrame.destination][1] = dw.getTXTimestamp();  //Save the second timestamp on the receiving node/anchor (T_sr)
            break;
        case BEACON_RESPONSE:
            senderTimestamps[rangingFrame.destination][2] = dw.getTXTimestamp();    //Save the third timestamp on the sending node/beacon (T_sr)
            correctSenderTimestamps(rangingFrame.destination);                      //Correct the timestamps for the case of a counter overflow
            break;
        default:
            break;
    }

}

/**
 *  Get the distance to the Anchor with address @param destination.
 *
 *   @param destination The address of the anchor
 */
void DecaWaveNetwork::requestRanging(uint8_t destination)
{
    acknowledgement[destination] = false;
    float time_before = LocalTimer.read();

    sendPingFrame(destination);

    while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.02f)); // wait for succeeding ranging or timeout

    roundtriptimes[destination] = LocalTimer.read() - time_before;

    if(acknowledgement[destination]) {
        distances[destination] = calibratedDistance(destination);
    } else {
        distances[destination] = -1;
    }
}

inline float DecaWaveNetwork::calibratedDistance(uint8_t destination)
{

    float rawDistance = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);

// Least Squares calibration parameters determined from dynamic data on quadrotor with Optitrack
    float calibDistance = 0.9710*rawDistance - 0.5075;

// Calibration for Nucleo 0 (and 1)

//   if (this->address == 1) rawDistance+= 10;
//    switch(destination){
//        case 2:
//            return  rawDistance * 0.9754 - 0.5004;
//        case 3:
//            return  rawDistance * 0.9759 - 0.4103;
//        case 4:
//            return  rawDistance * 0.9798 - 0.5499;
//        case 5:
//            return  rawDistance * 0.9765 - 0.5169;
//        }

    return calibDistance;

}

void DecaWaveNetwork::requestRangingAll()
{
    for (int i = 1; i <= 4; i++) {  // Request ranging to all anchors
        requestRanging(i);
    }
}

void DecaWaveNetwork::sendPingFrame(uint8_t destination)
{
    rangingFrame.source = address;
    rangingFrame.destination = destination;
    rangingFrame.type = PING;
    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
}

void DecaWaveNetwork::sendTransferFrame(uint8_t destination, int timeDiffsReceiver)
{
    transferFrame.source = address;
    transferFrame.destination = destination;
    transferFrame.type = TRANSFER_FRAME;
    transferFrame.signedTime =  timeDiffsReceiver;                      //cast the time difference
    dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame));
}

void DecaWaveNetwork::sendDelayedAnswer(uint8_t destination, uint8_t type, uint64_t rxTimestamp)
{

    rangingFrame.source = address;
    rangingFrame.destination = destination;
    rangingFrame.type = type;

    if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40)
        dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40);
    else
        dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
}

void DecaWaveNetwork::correctReceiverTimestamps(uint8_t source)
{

    if(receiverTimestamps[source][0] > receiverTimestamps[source][1]) {
        receiverTimestamps[source][1] += MMRANGING_2POWER40;
        receiverTimestamps[source][2] += MMRANGING_2POWER40;
    }

    if(receiverTimestamps[source][1] > receiverTimestamps[source][2]) {
        receiverTimestamps[source][2] += MMRANGING_2POWER40;
    }

}

void DecaWaveNetwork::correctSenderTimestamps(uint8_t source)
{

    if (senderTimestamps[source][0] > senderTimestamps[source][1]) {
        senderTimestamps[source][1] += MMRANGING_2POWER40;
        senderTimestamps[source][2] += MMRANGING_2POWER40;
        overflow = true;
    } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
        senderTimestamps[source][2] += MMRANGING_2POWER40;
        overflow = true;
    } else overflow = false;

}
void DecaWaveNetwork::sendMessage(uint8_t *msg,uint8_t length)
{
    dw.sendFrame(msg,length);
}

void DecaWaveNetwork::parseMavlinkMsg(uint8_t msg_len)
{

    //char buf[1024];
    uint8_t byte;
    mavlink_message_t msg;
    mavlink_status_t status;


    for(int i=0; i<msg_len; i++) {
        memcpy(&byte,&mavlink_buffer[i],1);

        if(mavlink_parse_char(MAVLINK_COMM_0,byte,&msg,&status)) {

            //printf("Parse Successfully msg id %d\r\n",msg.msgid);
            if(msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
                mavlink_heartbeat_t hb_msg;
                mavlink_msg_heartbeat_decode(&msg,&hb_msg);
                //printf("System ID %d Comp ID %d \r\n",msg.sysid,msg.compid);
                nodes_in_range[msg.sysid]= msg.compid;
                last_heartbeat[msg.sysid]=LocalTimer.read();                
                //    dwm_LED = !dwm_LED;
            }
            /* if(msg.msgid==MAVLINK_MSG_ID_FUSED_IMU) {

                 mavlink_fused_imu_t imu_msg;
                 // sendMessage(&pc,buf,n);
                 mavlink_msg_fused_imu_decode(&msg,&imu_msg);
                 imu_time = imu_msg.time_boot_ms;
                 roll = imu_msg.roll;
                 pitch = imu_msg.pitch;
                 yaw = imu_msg.yaw;
                 gx = imu_msg.gyro_x;
                 gy = imu_msg.gyro_y;
                 gz = imu_msg.gyro_z;
                 ax = imu_msg.accel_x;
                 ay = imu_msg.accel_y;
                 az = imu_msg.accel_z;

                 imu_LED = !imu_LED;
             }*/

            //This is from the perspective of the anchor node which doesn't compute the range
            if(msg.msgid == MAVLINK_MSG_ID_RANGE_TO_NODE) {
                mavlink_range_to_node_t rng_msg;
                mavlink_msg_range_to_node_decode(&msg,&rng_msg);

                if(rng_msg.tgt_id==address) { // if the range message is inteded for me
                    // rng_time = rng_msg.time_boot_ms;
                    ranges[rng_msg.my_id] = rng_msg.range; // my_id is the id of the sender
                    
                }

            }

            if(msg.msgid == MAVLINK_MSG_ID_LOCAL_POSITION_NED) {
                mavlink_local_position_ned_t pose_msg;
                mavlink_msg_local_position_ned_decode(&msg,&pose_msg);            
                //printf("System ID %d Comp ID %d \r\n",msg.sysid,msg.compid);                
                node_pose[msg.sysid] = pose_msg;

            }            
            
            

        }// if Mavlink Parse Returns true

    }// End For number of bytes
}

void DecaWaveNetwork::checkConnectivity(){
    printf("connectivity check\r\n");
    for(int i=0;i<MAX_NODES;i++){
        if(LocalTimer.read()-last_heartbeat[i] > 5.0){ //if its been more than 5 seconds since we recieved a heartbeat assume node is out of range
            nodes_in_range[i] = 0;
        }
    }
}
float DecaWaveNetwork::getRange(int node){
    return ranges[node];   
}