source: flair-src/trunk/lib/FlairSensorActuator/src/Bldc_impl.cpp

// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2013/11/14
//  filename:   Bldc_impl.cpp
//
//  author:     Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Virtual class for brushless drivers
//
//
/*********************************************************************/
#include "Bldc_impl.h"
#include "Bldc.h"
#include <GridLayout.h>
#include <GroupBox.h>
#include <DoubleSpinBox.h>
#include <Matrix.h>
#include <Label.h>
#include <PushButton.h>
#include <DataPlot1D.h>
#include <TabWidget.h>
#include <Tab.h>
#include <sstream>

using std::string;
using std::ostringstream;
using namespace flair::core;
using namespace flair::gui;
using namespace flair::actuator;

Bldc_impl::Bldc_impl(Bldc *self, Layout *layout, string name,
                     uint8_t motors_count) {
  this->self = self;
  this->motors_count = motors_count;
  this->layout = layout;
  are_enabled = false;
  is_running = false;
  tested_motor = -1;

  values = (float *)malloc(motors_count * sizeof(float));
  power = (float *)malloc(motors_count * sizeof(float));
  for (int i = 0; i < motors_count; i++)
    power[i] = 1;

  // station sol
  GroupBox *groupbox = new GroupBox(layout->NewRow(), "bldc");
  flight_time = new Label(groupbox->NewRow(), "flight time");
  min_value = new DoubleSpinBox(groupbox->NewRow(), "min value:", 0, 1, .1, 2);
  max_value =
      new DoubleSpinBox(groupbox->LastRowLastCol(), "max value:", 0, 1, .1, 2);
  test_value =
      new DoubleSpinBox(groupbox->LastRowLastCol(), "test value:", 0, 1, 0.1);

  int index = 0;
  button_test = (PushButton **)malloc(motors_count * sizeof(PushButton *));
  for (int i = 0; i < motors_count / 2; i++) {
    for (int j = 0; j < 2; j++) {
      ostringstream test_name;
      test_name << "test motor " << index;
      button_test[index] =
          new PushButton(groupbox->At(2 + i, j), test_name.str());
      index++;
    }
  }
  if(motors_count%2!=0) {
    ostringstream test_name;
    test_name << "test motor " << index;
    button_test[index] =new PushButton(groupbox->NewRow(), test_name.str());
    index++;
  }

  // flight time
  FILE *file;
  file = fopen("/etc/flight_time", "r");
  if (file == NULL) {
    time_sec = 0;
  } else {
    char ligne[32];
    fgets(ligne, 32, file);
    time_sec = atoi(ligne);
    fclose(file);
  }
  flight_time->SetText("total flight time: %is = %imin = %ih\n", time_sec,
                       time_sec / 60, time_sec / 3600);
}

Bldc_impl::~Bldc_impl() {
  if (values != NULL)
    free(values);
  if (button_test != NULL)
    free(button_test);
  if (power != NULL)
    free(power);
}

void Bldc_impl::UseDefaultPlot(TabWidget *tab) {
  Tab *plot_tab = new Tab(tab, "speeds");
  plots = new DataPlot1D(plot_tab->NewRow(), "values", 0, 1);
  for (int i = 0; i < motors_count; i++) {
    plots->AddCurve(self->output->Element(i));
  }
}

void Bldc_impl::UpdateFrom(const io_data *data) {
  Matrix *input = (Matrix *)data;
  bool is_motor_running = false;

  if (values == NULL)
    return; // nothing to do in simulator

  if (input->Rows() != motors_count) {
    self->Err("nb motors mismatch\n");
    return;
  }

  input->GetMutex();
  for (int i = 0; i < motors_count; i++) {
    if (are_enabled) {
      //Printf("%i %f %f\n",i,input->ValueNoMutex(i,0),power[i]);
      values[i] = power[i] * Sat(input->ValueNoMutex(i, 0));
      // Printf("%i %f\n",i,values[i]);
    } else {
      values[i] = 0;
    }
    if(values[i] != 0) is_motor_running = true;
  }
  input->ReleaseMutex();

  if (are_enabled && is_motor_running && !is_running) {
    flight_start_time = GetTime();
    is_running = true;
  }
  if ((!are_enabled || !is_motor_running) && is_running) {
    Time now = GetTime();
    int t_sec;
    FILE *file;
    char ligne[32];

    t_sec = (now - flight_start_time) / 1000000000;
    time_sec += t_sec;

    Printf("temps de vol: %is = %imin\n", t_sec, t_sec / 60);
    // Printf("temps de vol total: %is = %imin =
    // %ih\n",time_sec,time_sec/60,time_sec/3600);
    flight_time->SetText("total flight time: %is = %imin = %ih\n", time_sec,
                         time_sec / 60, time_sec / 3600);

    file = fopen("/etc/flight_time", "w");
    if (file == NULL) {
      Printf("Erreur a l'ouverture du fichier d'info vol\n");
    } else {
      sprintf(ligne, "%i", time_sec);
      fputs(ligne, file);
      fclose(file);
    }
    is_running = false;
  }

  for (int i = 0; i < motors_count; i++) {
    if (button_test[i]->Clicked() == true) {
      if (!are_enabled) {
        tested_motor = i;
        test_start_time = GetTime();
        LockUserInterface();
      } else {
        self->Warn("testing motor is not possible when enabled\n");
      }
    }
  }
  if (tested_motor != -1) {
    for (int i = 0; i < motors_count; i++) {
      values[i] = 0;
    }
    values[tested_motor] = test_value->Value();

    if (GetTime() > (test_start_time + 2 * 1000000000)) {
      tested_motor = -1;
      UnlockUserInterface();
    }
  }

  self->SetMotors(values);
  self->output->SetDataTime(data->DataTime());
  self->ProcessUpdate(self->output);
}

float Bldc_impl::Sat(float value) {
  float result = value;

  if (result < min_value->Value()) {
    result = min_value->Value();
  }
  if (result > max_value->Value()) {
    result = max_value->Value();
  }

  return result;
}

void Bldc_impl::LockUserInterface(void) const { layout->setEnabled(false); }

void Bldc_impl::UnlockUserInterface(void) const { layout->setEnabled(true); }