def test_up(self):
        speeds = AxisSpeeds()
        speeds.axis_rotation_speeds[0] = 0 # roll axis
        speeds.axis_rotation_speeds[1] = 0 # yaw axis
        speeds.axis_rotation_speeds[2] = 0 # pitch axis
        speeds.vertical_speed = 1 # up

        self.assertEqual(len(speeds.asArray()), 4)
        self.assertTrue((speeds.asArray() == numpy.array([0, 0, 0, 1])).all())

        # expected Result: zero matrix
        self.assertEqual(len(speeds.toMotorSignals()), 4)
        self.assertTrue((speeds.toMotorSignals() == numpy.array([0.25, 0.25, 0.25, 0.25])).all())
    def step(self, time_delta, quadro_state):

        # --------------------------------------------------
        # Axis rotation planning for all 3 axis
        rotPlans = quadro_state.rotation.plan_change_to(self.target_state.rotation).in_seconds(TIME_TO_REACH_TARGET_STATE)

        # --------------------------------------------------
        # do elevation planning for altitude
        elevationPlan = quadro_state.elevation.plan_change_to(self.target_state.elevation).in_seconds(TIME_TO_REACH_TARGET_STATE)

        # --------------------------------------------------
        # Prepare inputs for PIDs

        # calculate the target rotation speeds for the very next update
        targetRotSpeeds = map(
            lambda plan: plan.calculate_target_speed_for_time_period(0, TIME_BETWEEN_CONTROL_LOOP_UPDATES),
            rotPlans
        )
        # calculate the target elevation speed for the very next update
        targetElevationSpeed = elevationPlan.calculate_target_speed_for_time_period(0, TIME_BETWEEN_CONTROL_LOOP_UPDATES)

        #--------------------------------------------------
        # Update PIDs
        # At the moment we only use the first plan segments above, because we always
        # do a replanning. We could break the control loop into two loops:
        # fast PIDs and slower state change planning. Then, also the other plan
        # segments would be used. For example, a replanning could be done
        # every 0.1 seconds and the PIDs could adjust the motors every 0.01 seconds.
        axisSpeeds = AxisSpeeds()
        axisSpeeds.axis_rotation_speeds[0] = self.xRotPID.update(quadro_state.rotation.x, targetRotSpeeds(0), time_delta)
        axisSpeeds.axis_rotation_speeds[1] = self.yRotPID.update(quadro_state.rotation.y, targetRotSpeeds(1), time_delta)
        axisSpeeds.axis_rotation_speeds[2] = self.zRotPID.update(quadro_state.rotation.z, targetRotSpeeds(2), time_delta)
        axisSpeeds.vertical_speed = self.vertSpdPID.update(quadro_state.elevationState, targetElevationSpeed, time_delta)

        # transform to motor speeds and send to motors
        self.motors.setSpeed(axisSpeeds.toMotorSignals())