> Then the Mosfet has a Vgs of only 3V and is barely turned on.
There are legions of MOSFETs which can be fully turned on with Vgs at 2.5, or even 1.8V. Selecting the right MOSFET saves an amplifier. Using a BJT instead wound include the base current in the current measurement, if the BJT saturates, precision can become pretty bad. But an unsaturated, highish β BJT will do the job too.
> What's the capacitor for on the mosfet gate? That's just going to slow down the switching time.
The MOSFET is used in analog current source mode, so it is not switching. It is a RC to turn a PWM into a filtered DC.
> Place the current sense resistor on the high side of the mosfet.
Yes, you can do that, allowing use of a 5V MOSFET (or a low-β BJT as a dumb current amplifier)
To make the MOSFET switch cleanly at 40 kHz, you'd need a proper driver, Microchip makes some small cheap drivers, but I wanted to propose a solution doable with junk bin parts (although one could say the 2.5V MOSFET doesn't qualify
but a 1A, high β BJT would) and that doesn't need synchronized sampling (hence the analog filtering).
Another one, this time, much more efficient, and over-engineered, too :
Online Sketch: Untitled Sketch by Anonymous Author