wolrd velocty control in simulation

[SeaingAnt]

I'm trying to control the crazyflie in the simulation by sending global velocity. I added the following method in the crazyflie_sil but it does not have an effect as the drone falls whatever twist I give.

def cmdVelocityWorld(self, vel, yawRate):
        self.mode = CrazyflieSIL.MODE_LOW_VELOCITY
        self.setpoint = firm.setpoint_t()
        self.setpoint.velocity.x = vel[0]
        self.setpoint.velocity.y = vel[1]
        self.setpoint.velocity.z = vel[2]
        self.setpoint.velocity_body=True
        self.setpoint.mode.x = firm.modeVelocity
        self.setpoint.mode.y = firm.modeVelocity
        self.setpoint.mode.z = firm.modeVelocity
        self.setpoint.attitudeRate.yaw = yawRate
        self.setpoint.mode.yaw = firm.modeVelocity
        self.cmdHl_vel = copy_svec(self.setpoint.velocity)

This method should be similar to the one in the firmware crtp_commander_generic.c

struct velocityPacket_s {
  float vx;        // m in the world frame of reference
  float vy;        // ...
  float vz;        // ...
  float yawrate;  // deg/s
} __attribute__((packed));
static void velocityDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
{
  const struct velocityPacket_s *values = data;

  ASSERT(datalen == sizeof(struct velocityPacket_s));

  setpoint->mode.x = modeVelocity;
  setpoint->mode.y = modeVelocity;
  setpoint->mode.z = modeVelocity;

  setpoint->velocity.x = values->vx;
  setpoint->velocity.y = values->vy;
  setpoint->velocity.z = values->vz;

  setpoint->mode.yaw = modeVelocity;

  setpoint->attitudeRate.yaw = -values->yawrate;
}

do you have any idea what is wrong?

[whoenig]

Our SIL does not simulate the whole firmware, just parts of it. I believe that there are more changes needed to support CrazyflieSIL.MODE_LOW_VELOCITY properly. In any case, you would need to enable the physics np backend, since the default none backend can only operate with position setpoints.

[SeaingAnt]

thanks, I'm actually using np backend. The CrazyflieSIL.MODE_LOW_VELOCITY has no effect like the cmd full state mode. By setting the setpoint, cmdVelocityWorld works like cmd full state already implemented in the crazyflie_sitl. This setpoint is given directly to the controller like in the firmware. For this reason, I am expecting a similar implementation to the world velocity in the firmware. Any idea why it doesn't work?

[SeaingAnt]

Update:

I tried to run the ros2 node cmd_full_state.py in simulation and, after the takeoff, the drone crashed on the ground. Is this normal?

[whoenig]

cmdVelocityWorld: Which controller are you using? It's possible that only the PID controller internally can actually handle velocity control. Mellinger does not have support for this mode as far as I know.

cmd_full_state.py crash: There is an open bug in #297, which might be related and that I haven't looked at in more detail, yet.

[SeaingAnt]

The problem was these two lines in the executeController

        if self.mode != CrazyflieSIL.MODE_HIGH_POLY:
            return sim_data_types.Action([0, 0, 0, 0])

if commented out everything works fine