Used for sending control setpoints to the Crazyflie
| 43 | |
| 44 | |
| 45 | class Commander(): |
| 46 | """ |
| 47 | Used for sending control setpoints to the Crazyflie |
| 48 | """ |
| 49 | |
| 50 | def __init__(self, crazyflie=None): |
| 51 | """ |
| 52 | Initialize the commander object. By default the commander is in |
| 53 | +-mode (not x-mode). |
| 54 | """ |
| 55 | self._cf = crazyflie |
| 56 | self._x_mode = False |
| 57 | |
| 58 | def set_client_xmode(self, enabled): |
| 59 | """ |
| 60 | Enable/disable the client side X-mode. When enabled this recalculates |
| 61 | the setpoints before sending them to the Crazyflie. |
| 62 | """ |
| 63 | self._x_mode = enabled |
| 64 | |
| 65 | def send_setpoint(self, roll, pitch, yaw, thrust): |
| 66 | """ |
| 67 | Send a new control setpoint for roll/pitch/yaw/thrust to the copter |
| 68 | |
| 69 | The arguments roll/pitch/yaw/trust is the new setpoints that should |
| 70 | be sent to the copter |
| 71 | """ |
| 72 | if thrust > 0xFFFF or thrust < 0: |
| 73 | raise ValueError('Thrust must be between 0 and 0xFFFF') |
| 74 | |
| 75 | if self._x_mode: |
| 76 | roll, pitch = 0.707 * (roll - pitch), 0.707 * (roll + pitch) |
| 77 | |
| 78 | pk = CRTPPacket() |
| 79 | pk.port = CRTPPort.COMMANDER |
| 80 | pk.data = struct.pack('<fffH', roll, -pitch, yaw, thrust) |
| 81 | self._cf.send_packet(pk) |
| 82 | |
| 83 | def send_stop_setpoint(self): |
| 84 | """ |
| 85 | Send STOP setpoing, stopping the motors and (potentially) falling. |
| 86 | """ |
| 87 | pk = CRTPPacket() |
| 88 | pk.port = CRTPPort.COMMANDER_GENERIC |
| 89 | pk.data = struct.pack('<B', TYPE_STOP) |
| 90 | self._cf.send_packet(pk) |
| 91 | |
| 92 | def send_velocity_world_setpoint(self, vx, vy, vz, yawrate): |
| 93 | """ |
| 94 | Send Velocity in the world frame of reference setpoint. |
| 95 | |
| 96 | vx, vy, vz are in m/s |
| 97 | yawrate is in degrees/s |
| 98 | """ |
| 99 | pk = CRTPPacket() |
| 100 | pk.port = CRTPPort.COMMANDER_GENERIC |
| 101 | pk.data = struct.pack('<Bffff', TYPE_VELOCITY_WORLD, |
| 102 | vx, vy, vz, yawrate) |