Added sensor message to firmware and package
fixed bug
fixed bugs
fixing bug of the firmware