def __init__(self, vehicle_event_dispatcher, ui_event_dispatcher):
QtGui.QWidget.__init__(self)
BasePanelController.__init__(self)
self.ui = Ui_AccelCalibrationPanel()
self.ui.setupUi(self)
self.ui.start_button.clicked.connect(self._start_button_pressed)
self.ui.cancel_button.clicked.connect(self._cancel_button_pressed)
self.ui.progress_bar.setMaximum(
AccelCalibrationController.NB_SAMPLE_TO_READ)
ui_event_dispatcher.register(self._protocol_handler_changed_event,
UIEventDispatcher.PROTOCOL_HANDLER_EVENT)
vehicle_event_dispatcher.register(
self._accel_raw_data_received,
VehicleEventDispatcher.ACCEL_RAW_DATA_EVENT)
self._set_initial_panel_state()
def __init__(self, vehicle_event_dispatcher, ui_event_dispatcher):
QtGui.QWidget.__init__(self)
BasePanelController.__init__(self)
self.ui = Ui_AccelCalibrationPanel()
self.ui.setupUi(self)
self.ui.start_button.clicked.connect(self._start_button_pressed)
self.ui.cancel_button.clicked.connect(self._cancel_button_pressed)
self.ui.progress_bar.setMaximum(AccelCalibrationController.NB_SAMPLE_TO_READ)
ui_event_dispatcher.register(self._protocol_handler_changed_event, UIEventDispatcher.PROTOCOL_HANDLER_EVENT)
vehicle_event_dispatcher.register(self._accel_raw_data_received, VehicleEventDispatcher.ACCEL_RAW_DATA_EVENT)
self._set_initial_panel_state()
class AccelCalibrationController(QtGui.QWidget, BasePanelController):
LEVELLED_CALIBRATION_STEP_ID = 0
UPSIDE_DOWN_CALIBRATION_STEP_ID = 1
LEFT_SIDE_CALIBRATION_STEP_ID = 2
RIGHT_SIDE_CALIBRATION_STEP_ID = 3
NOSE_UP_CALIBRATION_STEP_ID = 4
NOSE_DOWN_CALIBRATION_STEP_ID = 5
COMPLETE_ACCEL_CALIBRATION_STEP_ID = 6
START_TEXT = 'Start'
NEXT_TEXT ='Next >>'
COMPLETE_TEXT = 'Complete'
NB_SAMPLE_TO_READ = 50
ONE_G = 9.80665
def __init__(self, vehicle_event_dispatcher, ui_event_dispatcher):
QtGui.QWidget.__init__(self)
BasePanelController.__init__(self)
self.ui = Ui_AccelCalibrationPanel()
self.ui.setupUi(self)
self.ui.start_button.clicked.connect(self._start_button_pressed)
self.ui.cancel_button.clicked.connect(self._cancel_button_pressed)
self.ui.progress_bar.setMaximum(AccelCalibrationController.NB_SAMPLE_TO_READ)
ui_event_dispatcher.register(self._protocol_handler_changed_event, UIEventDispatcher.PROTOCOL_HANDLER_EVENT)
vehicle_event_dispatcher.register(self._accel_raw_data_received, VehicleEventDispatcher.ACCEL_RAW_DATA_EVENT)
self._set_initial_panel_state()
def _start_button_pressed(self):
if self.ui.start_button.text() == AccelCalibrationController.START_TEXT :
self.ui.start_button.setText(AccelCalibrationController.NEXT_TEXT)
self.ui.start_button.setEnabled(False)
self.ui.cancel_button.setEnabled(True)
self._protocol_handler.subscribe_raw_accelerometer()
elif self.ui.start_button.text() == AccelCalibrationController.NEXT_TEXT :
self._protocol_handler.subscribe_raw_accelerometer()
self.ui.start_button.setEnabled(False)
else :
self._send_calibration_score()
self._set_initial_panel_state()
def _cancel_button_pressed(self):
self._protocol_handler.unsubscribe_command()
self._set_initial_panel_state()
def _protocol_handler_changed_event(self, event, protocol_handler):
self._protocol_handler = protocol_handler;
def _accel_raw_data_received(self, event, accel_raw_data_vector):
if self._current_calibration_step == AccelCalibrationController.LEVELLED_CALIBRATION_STEP_ID or \
self._current_calibration_step == AccelCalibrationController.UPSIDE_DOWN_CALIBRATION_STEP_ID :
self._calibration_raw_sum_values[self._current_calibration_step] = \
self._calibration_raw_sum_values[self._current_calibration_step] + accel_raw_data_vector.get_z()
elif self._current_calibration_step == AccelCalibrationController.LEFT_SIDE_CALIBRATION_STEP_ID or \
self._current_calibration_step == AccelCalibrationController.RIGHT_SIDE_CALIBRATION_STEP_ID :
self._calibration_raw_sum_values[self._current_calibration_step] = \
self._calibration_raw_sum_values[self._current_calibration_step] + accel_raw_data_vector.get_y()
else :
self._calibration_raw_sum_values[self._current_calibration_step] = \
self._calibration_raw_sum_values[self._current_calibration_step] + accel_raw_data_vector.get_x()
self._current_nb_sampled_read = self._current_nb_sampled_read + 1
self.ui.progress_bar.setValue(self._current_nb_sampled_read)
if self._current_nb_sampled_read >= AccelCalibrationController.NB_SAMPLE_TO_READ:
self._proceed_to_next_calibration_step();
self._protocol_handler.unsubscribe_command()
def _proceed_to_next_calibration_step(self):
self._current_nb_sampled_read = 0
self._current_calibration_step = self._current_calibration_step + 1
self._update_panel_to_current_calibration_stage()
self.ui.start_button.setEnabled(True)
def start(self):
self._protocol_handler.unsubscribe_command()
self._set_initial_panel_state()
def stop(self):
self._protocol_handler.unsubscribe_command()
def _update_panel_to_current_calibration_stage (self):
pictureScene = QtGui.QGraphicsScene()
self.ui.start_button.setText(AccelCalibrationController.NEXT_TEXT)
if self._current_calibration_step == self.LEVELLED_CALIBRATION_STEP_ID:
pictureBackground = QtGui.QPixmap("./resources/callevel.png")
self.ui.information_display_text_box.setText("Place the AeroQuad on a flat and motionless surface and press the start to begin the calibration procedure")
elif self._current_calibration_step == self.UPSIDE_DOWN_CALIBRATION_STEP_ID:
pictureBackground = QtGui.QPixmap("./resources/callevel.png")
self.ui.information_display_text_box.setText("Place the AeroQuad upside down and press start")
elif self._current_calibration_step == self.LEFT_SIDE_CALIBRATION_STEP_ID:
pictureBackground = QtGui.QPixmap("./resources/calleft.png")
self.ui.information_display_text_box.setText("Place the AeroQuad left edge down and press start")
elif self._current_calibration_step == self.RIGHT_SIDE_CALIBRATION_STEP_ID:
pictureBackground = QtGui.QPixmap("./resources/calright.png")
self.ui.information_display_text_box.setText("Place the AeroQuad right edge down and press start")
elif self._current_calibration_step == self.NOSE_UP_CALIBRATION_STEP_ID:
pictureBackground = QtGui.QPixmap("./resources/calfront.png")
self.ui.information_display_text_box.setText("Place the AeroQuad front edge down and press start. The arrow indicates the front of the AeroQuad")
elif self._current_calibration_step == self.NOSE_DOWN_CALIBRATION_STEP_ID:
pictureBackground = QtGui.QPixmap("./resources/calrear.png")
self.ui.information_display_text_box.setText("Place the AeroQuad rear edge down and press start. The arrow indicates the front of the AeroQuad")
elif self._current_calibration_step == self.COMPLETE_ACCEL_CALIBRATION_STEP_ID:
pictureBackground = QtGui.QPixmap("./resources/callevel.png")
self.ui.information_display_text_box.setText("Place the AeroQuad on a flat surface and motionless surface and press the complete button to complete the calibration")
self.ui.start_button.setText(AccelCalibrationController.COMPLETE_TEXT)
pictureItem = QtGui.QGraphicsPixmapItem(pictureBackground)
pictureScene.addItem(pictureItem)
self.ui.picture_container.setScene(pictureScene)
def _set_initial_panel_state(self):
self._calibration_raw_sum_values = [0,0,0,0,0,0]
self._current_calibration_step = 0
self._current_nb_sampled_read = 0
self._update_panel_to_current_calibration_stage()
self.ui.start_button.setEnabled(True)
self.ui.start_button.setText(AccelCalibrationController.START_TEXT)
self.ui.cancel_button.setEnabled(False)
self.ui.progress_bar.setValue(0)
def _send_calibration_score(self):
temp_z = (math.fabs(self._calibration_raw_sum_values[0]) + math.fabs(self._calibration_raw_sum_values[1])) / 2
temp_y = (math.fabs(self._calibration_raw_sum_values[2]) + math.fabs(self._calibration_raw_sum_values[3])) / 2
temp_x = (math.fabs(self._calibration_raw_sum_values[4]) + math.fabs(self._calibration_raw_sum_values[5])) / 2
z_scale_factor = AccelCalibrationController.ONE_G / (temp_z / AccelCalibrationController.NB_SAMPLE_TO_READ) #* -1
y_scale_factor = AccelCalibrationController.ONE_G / (temp_y / AccelCalibrationController.NB_SAMPLE_TO_READ) #* -1
x_scale_factor = AccelCalibrationController.ONE_G / (temp_x / AccelCalibrationController.NB_SAMPLE_TO_READ)
if self._calibration_raw_sum_values[1] < 0 :
z_scale_factor = z_scale_factor * -1
if self._calibration_raw_sum_values[2] < 0 :
y_scale_factor = y_scale_factor * -1
if self._calibration_raw_sum_values[4] < 0 :
x_scale_factor = x_scale_factor * -1
self._protocol_handler.set_accel_calibration_scale_factor(x_scale_factor, y_scale_factor, z_scale_factor)
class AccelCalibrationController(QtGui.QWidget, BasePanelController):
LEVELLED_CALIBRATION_STEP_ID = 0
UPSIDE_DOWN_CALIBRATION_STEP_ID = 1
LEFT_SIDE_CALIBRATION_STEP_ID = 2
RIGHT_SIDE_CALIBRATION_STEP_ID = 3
NOSE_UP_CALIBRATION_STEP_ID = 4
NOSE_DOWN_CALIBRATION_STEP_ID = 5
COMPLETE_ACCEL_CALIBRATION_STEP_ID = 6
START_TEXT = 'Start'
NEXT_TEXT = 'Next >>'
COMPLETE_TEXT = 'Complete'
NB_SAMPLE_TO_READ = 50
ONE_G = 9.80665
def __init__(self, vehicle_event_dispatcher, ui_event_dispatcher):
QtGui.QWidget.__init__(self)
BasePanelController.__init__(self)
self.ui = Ui_AccelCalibrationPanel()
self.ui.setupUi(self)
self.ui.start_button.clicked.connect(self._start_button_pressed)
self.ui.cancel_button.clicked.connect(self._cancel_button_pressed)
self.ui.progress_bar.setMaximum(
AccelCalibrationController.NB_SAMPLE_TO_READ)
ui_event_dispatcher.register(self._protocol_handler_changed_event,
UIEventDispatcher.PROTOCOL_HANDLER_EVENT)
vehicle_event_dispatcher.register(
self._accel_raw_data_received,
VehicleEventDispatcher.ACCEL_RAW_DATA_EVENT)
self._set_initial_panel_state()
def _start_button_pressed(self):
if self.ui.start_button.text(
) == AccelCalibrationController.START_TEXT:
self.ui.start_button.setText(AccelCalibrationController.NEXT_TEXT)
self.ui.start_button.setEnabled(False)
self.ui.cancel_button.setEnabled(True)
self._protocol_handler.subscribe_raw_accelerometer()
elif self.ui.start_button.text(
) == AccelCalibrationController.NEXT_TEXT:
self._protocol_handler.subscribe_raw_accelerometer()
self.ui.start_button.setEnabled(False)
else:
self._send_calibration_score()
self._set_initial_panel_state()
def _cancel_button_pressed(self):
self._protocol_handler.unsubscribe_command()
self._set_initial_panel_state()
def _protocol_handler_changed_event(self, event, protocol_handler):
self._protocol_handler = protocol_handler
def _accel_raw_data_received(self, event, accel_raw_data_vector):
if self._current_calibration_step == AccelCalibrationController.LEVELLED_CALIBRATION_STEP_ID or \
self._current_calibration_step == AccelCalibrationController.UPSIDE_DOWN_CALIBRATION_STEP_ID :
self._calibration_raw_sum_values[self._current_calibration_step] = \
self._calibration_raw_sum_values[self._current_calibration_step] + accel_raw_data_vector.get_z()
elif self._current_calibration_step == AccelCalibrationController.LEFT_SIDE_CALIBRATION_STEP_ID or \
self._current_calibration_step == AccelCalibrationController.RIGHT_SIDE_CALIBRATION_STEP_ID :
self._calibration_raw_sum_values[self._current_calibration_step] = \
self._calibration_raw_sum_values[self._current_calibration_step] + accel_raw_data_vector.get_y()
else:
self._calibration_raw_sum_values[self._current_calibration_step] = \
self._calibration_raw_sum_values[self._current_calibration_step] + accel_raw_data_vector.get_x()
self._current_nb_sampled_read = self._current_nb_sampled_read + 1
self.ui.progress_bar.setValue(self._current_nb_sampled_read)
if self._current_nb_sampled_read >= AccelCalibrationController.NB_SAMPLE_TO_READ:
self._proceed_to_next_calibration_step()
self._protocol_handler.unsubscribe_command()
def _proceed_to_next_calibration_step(self):
self._current_nb_sampled_read = 0
self._current_calibration_step = self._current_calibration_step + 1
self._update_panel_to_current_calibration_stage()
self.ui.start_button.setEnabled(True)
def start(self):
self._protocol_handler.unsubscribe_command()
self._set_initial_panel_state()
def stop(self):
self._protocol_handler.unsubscribe_command()
def _update_panel_to_current_calibration_stage(self):
pictureScene = QtGui.QGraphicsScene()
self.ui.start_button.setText(AccelCalibrationController.NEXT_TEXT)
if self._current_calibration_step == self.LEVELLED_CALIBRATION_STEP_ID:
pictureBackground = QtGui.QPixmap(
"./resources/calTop Up + Background v2.png")
self.ui.information_display_text_box.setText(
"Place the AeroQuad on a flat, level and motionless surface and press Start to begin the calibration procedure"
)
elif self._current_calibration_step == self.UPSIDE_DOWN_CALIBRATION_STEP_ID:
pictureBackground = QtGui.QPixmap(
"./resources/calTop Down + Background v2.png")
self.ui.information_display_text_box.setText(
"Place the AeroQuad upside down and press Next")
elif self._current_calibration_step == self.LEFT_SIDE_CALIBRATION_STEP_ID:
pictureBackground = QtGui.QPixmap(
"./resources/calLeft + Background v2.png")
self.ui.information_display_text_box.setText(
"Place the AeroQuad left edge down and press Next")
elif self._current_calibration_step == self.RIGHT_SIDE_CALIBRATION_STEP_ID:
pictureBackground = QtGui.QPixmap(
"./resources/calRight + Background v2.png")
self.ui.information_display_text_box.setText(
"Place the AeroQuad right edge down and press Next")
elif self._current_calibration_step == self.NOSE_UP_CALIBRATION_STEP_ID:
pictureBackground = QtGui.QPixmap(
"./resources/calFront + Background v2.png")
self.ui.information_display_text_box.setText(
"Place the AeroQuad front edge down and press Next. The arrow indicates the front of the AeroQuad"
)
elif self._current_calibration_step == self.NOSE_DOWN_CALIBRATION_STEP_ID:
pictureBackground = QtGui.QPixmap(
"./resources/calRear + Background v2.png")
self.ui.information_display_text_box.setText(
"Place the AeroQuad rear edge down and press Next. The arrow indicates the front of the AeroQuad"
)
elif self._current_calibration_step == self.COMPLETE_ACCEL_CALIBRATION_STEP_ID:
pictureBackground = QtGui.QPixmap(
"./resources/calTop Up + Background v2.png")
self.ui.information_display_text_box.setText(
"Place the AeroQuad on a flat, level and motionless surface and press Complete to finish the calibration procedure"
)
self.ui.start_button.setText(
AccelCalibrationController.COMPLETE_TEXT)
pictureItem = QtGui.QGraphicsPixmapItem(pictureBackground)
pictureScene.addItem(pictureItem)
self.ui.picture_container.setScene(pictureScene)
def _set_initial_panel_state(self):
self._calibration_raw_sum_values = [0, 0, 0, 0, 0, 0]
self._current_calibration_step = 0
self._current_nb_sampled_read = 0
self._update_panel_to_current_calibration_stage()
self.ui.start_button.setEnabled(True)
self.ui.start_button.setText(AccelCalibrationController.START_TEXT)
self.ui.cancel_button.setEnabled(False)
self.ui.progress_bar.setValue(0)
def _send_calibration_score(self):
temp_z = (math.fabs(self._calibration_raw_sum_values[0]) +
math.fabs(self._calibration_raw_sum_values[1])) / 2
temp_y = (math.fabs(self._calibration_raw_sum_values[2]) +
math.fabs(self._calibration_raw_sum_values[3])) / 2
temp_x = (math.fabs(self._calibration_raw_sum_values[4]) +
math.fabs(self._calibration_raw_sum_values[5])) / 2
z_scale_factor = AccelCalibrationController.ONE_G / (
temp_z / AccelCalibrationController.NB_SAMPLE_TO_READ) #* -1
y_scale_factor = AccelCalibrationController.ONE_G / (
temp_y / AccelCalibrationController.NB_SAMPLE_TO_READ) #* -1
x_scale_factor = AccelCalibrationController.ONE_G / (
temp_x / AccelCalibrationController.NB_SAMPLE_TO_READ)
if self._calibration_raw_sum_values[1] < 0:
z_scale_factor = z_scale_factor * -1
if self._calibration_raw_sum_values[2] < 0:
y_scale_factor = y_scale_factor * -1
if self._calibration_raw_sum_values[4] < 0:
x_scale_factor = x_scale_factor * -1
self._protocol_handler.set_accel_calibration_scale_factor(
x_scale_factor, y_scale_factor, z_scale_factor)
