pprz_doublet.c

Go to the documentation of this file.

/*
* Copyright (C) Alessandro Collicelli
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING.  If not, see
* <http://www.gnu.org/licenses/>.
*/
#include "pprz_doublet.h"
#include "std.h"
 
 
 
void doublet_init(struct doublet_t *doublet, float length_s, float extra_waiting_time_s, float current_time_s, uint8_t mod)
{
    doublet->t0 = current_time_s;
    doublet->tf = length_s;
    doublet->total_length_s = doublet->tf + extra_waiting_time_s;
    doublet->mod = mod;
    doublet->current_value = 0;
    doublet->current_time_s = current_time_s;
 
    if (mod==0) { // Normal doublet
        doublet->t1 = length_s / 4;
        doublet->t2 = doublet->t1 * 2;
        doublet->t3 = doublet->t1 * 3;
        doublet->t4 = doublet->t1 * 4;
        doublet->t5 = doublet->tf;
    } else if (mod==1) { // Half doublet
        doublet->t1 = length_s / 4;
        doublet->t2 = doublet->t1 * 2;
        doublet->t3 = doublet->t1 * 3;
        doublet->t4 = doublet->t1 * 4;
        doublet->t5 = doublet->tf;
    } else if (mod==2) { // 3211 doublet 
        doublet->t1 = length_s / 9;
        doublet->t2 = doublet->t1 * 4;
        doublet->t3 = doublet->t1 * 6;
        doublet->t4 = doublet->t1 * 7;
        doublet->t5 = doublet->t1 * 8;
    } else if (mod==3) { // highspeed doublet: one doublet, no waiting time
        doublet->t1 = 0;
        doublet->t2 = length_s / 2;
        doublet->t3 = doublet->tf;
        doublet->t4 = 0; // not used
        doublet->t5 = 0; // not used
    }
}
 
void doublet_reset(struct doublet_t *doublet, float current_time_s){
    doublet->t0 = current_time_s;
    doublet->current_value = 0.0f;
    doublet->current_time_s = current_time_s;
}
 
bool doublet_is_running(struct doublet_t *doublet, float current_time_s){
    float t = current_time_s - doublet->t0;
    return (t >= 0) && (t <= doublet->total_length_s);
}
 
float doublet_update(struct doublet_t *doublet, float current_time_s){
    
    if(!doublet_is_running(doublet, current_time_s)){
        doublet->current_value = 0.0f;
        return 0;
    }
 
    float t = current_time_s - doublet->t0; // since the start of the doublet
    if ((t>=0) & (t<=doublet->tf)){
        if (doublet->mod == 0 || doublet->mod == 3) {
            if((t >= doublet->t1) & (t <= doublet->t2)){
                doublet->current_value = 1.0f;
            }else if((t >= doublet->t2) & (t <= doublet->t3)){
                doublet->current_value = -1.0f;
            }else{
                doublet->current_value = 0.0f;
            }
        } else if (doublet->mod == 1) {
            if((t >= doublet->t1) & (t <= doublet->t2)){
                doublet->current_value = 1.0f;
            }else if((t >= doublet->t2) & (t <= doublet->t3)){
                doublet->current_value = 1.0f;
            }else{
                doublet->current_value = 0.0f;
            }
        } else if (doublet->mod == 2) {
            if (((t >= doublet->t1) & (t <= doublet->t2)) | ((t >= doublet->t3) & (t <= doublet->t4))){
                doublet->current_value = 1.0f;
            }else if(((t >= doublet->t2) && (t <= doublet->t3)) | ((t >= doublet->t4) && (t <= doublet->t5))){
                doublet->current_value = -1.0f;
            }else{
                doublet->current_value = 0.0f;
            }
        }
    } else {
        doublet->current_value = 0.0f;
    }
    return doublet->current_value;
}